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X Abstract 

^Application  of  the  Kalman  Filter  to  the  fire  control  problem  is 
considered.  While  the  theory  and  practice  in  this  field  are  well 
developed,  conflicting  claims  have  been  made  regarding  the  relative 
advantages  of  statistical  decoupl ing  in  fixed  inertial  coordinates 
as  compared  to  rotating  non-lnertial  coordinates.  An  error  analysis 
model,  consisting  of  a sub-optimal  filter  and  truth  model,  shows  the  error 
added,  in  each  of  three  formulations,  due  to  several  sub-optimal 
approximations.  Plots,  Including  error  ellipses,  help  to  provide  an 
intuitive  feel  for  the  results  of  decoupling.  ^ 


II.  Introduction 


The  primary  purpose  of  an  anti-aircraft  gun  fire  control  computer 
Is  to  compute  gun  direction  Information  In  order  to  maximize  the 
probability  of  hitting  the  target.  Specifically,  lead  angles  are 
computed  which  anticipate  target  motion  during  projectile  time  of 
flight.  During  an  encounter,  time  series  of  redundant  measurements 
are  available  from  such  devices  as  radar  and  inertial  position  and 
rate  sensors.  Most  current  fire  control  computers  contain  a Kalman 
Filter  target  state  estimator.  This  mathematical  algorithm  makes  an  *■ 

approximately  optimal  real  time  estimate  of  target  position,  velocity,  1* 

and  acceleration  from  which  lead  angles  are  computed. 

A full  scale  implementation  of  a Kalman  Filter  to  the  fire  control 
problem  is  quite  complex.  The  problem  must  be  solved  in  three  dimensions 
and  in  real  time.  A target  model  must  in  some  way  help  to  predict  future  i 

maneuvers.  To  this  end,  some  algorithms  have  adaptive  target  models. 

Furthermore,  the  optimality  of  the  solution  require  a careful  accounting 
of  error  sources.  All  of  these,  and  other,  requirements  cannot  be  met 
by  present  computers.  The  most  effective  reduction  of  computational 
load  with  moderate  loss  in  accuracy  follows  from  a statistical  decoupling 
of  errors  among  the  computational  axes. 

It  is  the  purpose  of  this  study  to  compare  the  added  error  due  to 
sub-optimal  approximations  for  three  fire  control  formulations.  Of 
primary  interest  is  the  error  added  due  to  decoupling.  The  three 
formulations  which  are  compared  are  as  follows: 
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A.  Using  rate  gyros  as  a reference,  computation  is  carried 


out  in  a rotating  tracker,  or  line  of  sight  (LOS),  frame. 

B.  Using  an  inertial  measurement  unit  (IMU)  as  a reference, 
computation  is  carried  out  in  LOS  coordinates. 

C.  Using  an  IMU  reference,  computation  is  carried  out  in 
inertial  (I)  coordinates. 

While  formulations  A and  C have  been  implemented  in  existing  systems, 
formulation  B is  considered  here  as  an  alternative  where  software  similar 
to  that  of  A could  be  implemented  with  the  hardware  of  C.  *■ 

In  order  to  limit  the  computation  and  to  clearly  show  the  effects 
of  decoupling,  the  overall  fire  control  problem  is  simplified  in  the 
following  way.  Only  two  dimension  are  considered,  with  the  elimination 
of  the  vertical  dimension.  A simple,  non-adaptive  Markov  model  represents 
the  uncertainty  in  target  motion.  Finally,  only  non-accelerating  straight 
line  target  trajectories  are  considered. 


III.  Summary  of  Results  and  Conclusions 


In  general,  it  was  found  that  radar  measurements,  being  made  along 
the  LOS  coordinate  axes,  tend  to  align  position  error  ellipses,  and  to 
a lesser  extent,  velocity  ellipses,  to  the  LOS  axes.  This  causes  the 
LOS  covariance  terms  which  are  eliminated  by  decoupling  to  be  small 
in  the  first  place.  Hence,  the  effect  due  to  decoupling  on  LOS  covariance 
is  relatively  minor.  On  the  other  hand,  since  LOS  axes  are  rotating  with 
respect  to  the  I axes,  the  I cross  terms  are  not  necessarily  small,  and 
decoupling  can  have  a considerable  effect  on  I covariance. 

For  the  above  reason,  the  error  added  to  A and  B due  to  decoupling  is 
less  than  that  added  to  C.  However,  for  reasons  discussed  in  detail 
in  Chapter  VI,  the  reduction  in  error  in  B relative  to  C is  not  great, 
so  based  upon  the  cases  studied,  it  would  not  appear  that  a shift  in 
software  to  convert  C to  B would  be  justified.  But,  if  a system  were 
in  the  design  stages,  based  upon  the  relatively  inexpensive  rate  gyro 
as  compared  to  an  IMl,  it  would  appear  that  formulation  A would  be  pre- 
ferred. 

Care  should  be  taken  to  interpret  the  results  for  what  they  are: 

Having  prepared  a reasonably  extensive  set  of  computer  programs,  time 
permitted  the  running  of  a limited  number  of  cases.  Two  major  questions 
remain  open.  First,  to  what  extent  do  the  results  obtained  generalize 
to  a wider  class  of  target  trajectories?  The  second  question  is 
tantalizing.  In  marked  contrast  to  I decoupling,  it  was  found  that  the 
Kalman  gain  terms  not  zeroed  by  LOS  decoupling  are  nearly  undisturbed. 
Furthermore,  the  LOS  Kalman  gains  are  to  a large  extent  trajectory 
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independent.  The  question  is  whether  simple  empirical  expressions 
can  be  developed  to  replace  the  lost  terms,  further  reducing  the  penalty 
due  to  LOS  decoupling.  Further  study  is  required  to  develop  a definitive 
conclusion  to  the  decoupling  question. 
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IV.  Formulation  Development 


A.  Introduction 

There  are  quite  a number  of  target  state  estimator  formulations, 
all  of  which  are  designed  to  accomplish  the  goal  of  producing  real  time 
estimates  of  target  position,  velocity,  and  acceleration.  The  purpose 
is,  of  course,  to  form  the  initial  conditions  of  the  equations  which 
will  project  target  position  ahead  in  time  to  projectile  impact,  thus 
forming  gun  lead  angles.  Differences  from  one  formulation  to  another 
cause  differences  in  accuracy  and  computer  loading. 

A few  of  the  more  obscure  formulations  are  worth  mentioning. 

f i 

In  one  case,  the  equations  of  motion  are  written  in  spherical  coordinates 
so  that  the  radar  measurements  are  strictly  linear.  This  leads  to  badly 
non-linear  equations  of  motion  which  can  be  ill-conditioned,  and  difficult 
to  integrate.  To  alleviate  this  problem,  one  author  resorted  to  a highly 
unorthodox  linearization  technique.  Another  approach  has  been  to  work 
entirely  in  cartesian  coordinate  where  the  equations  of  motion  are 
linear,  but  the  measurements  are  non-linear. 

As  target  state  estimation  has  evolved,  a more  or  less  standard 
approach  has  been  to  write  the  equations  of  motion  in  fixed  cartesian 
coordinates.  Approximately  linear  measurements  are  made  in  a tracker 
fixed  frame,  then  transformed  to  the  computational  frame. 

An  alternative  approach  is  to  compute  in  a rotating  cartesian 
frame  fixed  to  the  tracker.  In  this  case,  linearity  has  been  maintained, 
and  the  need  for  transformation  has  been  eliminated. 

These  last  two  approaches  have  been  both  implemented,  and 
proponents  of  each  claim  advantages  of  one  in  comparison  to  the  other. 
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It  is  the  purpose  of  this  study  to  simulate  both  of  the  latter  formu- 
lations in  the  error  analysis  mode  in  an  attempt  to  resolve  some  of  the 
conflicting  claims. 

The  formulation  using  a fixed  inertial  (I)  reference  frame  is 
established  by  an  inertial  measurement  unit  (IMU)  and  will  be  termed 
pure  inertial.  The  formulation  using  rotating  cartesian  coordinates, 
aligned  to  the  tracker  line  of  sight  (LOS)  will  be  termed  pure  LOS. 

While  pure  LOS  uses  rate  gyros  to  establish  rotation  rates,  a third 
formulation  will  be  considered  which  derives  rates  from  an  IMU,  and  will 
be  termed  a hybrid  formulation.  The  three  formulations  under  consider- 
ation are  summarized  below. 


Formulation 

Pure 

LOS  (A) 

Hybrid  (B) 

Pure 

Inertial  (C) 

Propagation  of  State 

LOS 

I 

I 

Propagation  of  Variance 

LOS 

LOS 

I 

Residual  Formation 

LOS 

LOS 

LOS 

Reference 

Rate  Gyros 

IMU 

IMU 

TABLE  IV-1 

An  ultimate  goal  of  the  study  is  to  compare  decoupling  in  LOS  to  I. 

It  is  for  this  reason  that  propagation  of  variance  in  (B)  is  in  LOS.  How- 
ever, given  the  presence  of  an  IMU  in  (B),  it  is  more  accurate  to  propagate 
state  in  I.  In  all  cases  the  residual  is  formed  in  the  LOS  frame.  Cross 
range  error  consists  of  the  error  due  to  radar  servo  mistracking.  Because 
these  angular  errors  are  quite  small  the  measurements  are  very  nearly  linear. 

The  remaining  sections  of  chapter  IV  consist  of  a detailed  develop- 
ment of  formulations  A,  B,  and  C. 
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Figure  IV-1 

The  (x.y)  frame  Is  fixed  (I),  where  the  target  position  and 

velocity  are  (0  , D ) and  V , V ) respectively, 
x y x y 

The  ( s , v ) frame  (LOS)  is  rotated  through  an  angle.  A,  relative 
to  (1),  and  Is  rotating  at  a rate  A.  Target  position  and  velocity  are 
(Ps.  and  Vs,  Vv)  and  are  inertial  quantities  resolved  onto  these  axes 
1 . State  Vectors 

<»  Sl-*C»x0y  Vy*«  */  <') 

(LOS)  XL_ -CDs  D*  Vs  Vt  as  d (]T  (2) 

The  study  is  carried  out  in  two  dimensions  for  the  sake  of 
simplicity.  The  extension  to  the  third  dimension  is  straightforward. 


2.  Transformation  Matrices 


From  Figure  1 , the  Euler  rotation  which  transforms  (LOS) 


to  ( I ) is  as  follows: 


cos  A -sin  Al  |D 


sin  A cos  AD 


Or,  due  to  the  orthogonality  of  the  matrix. 


Define 


cos  A sin  A]  |D 


sin  A cos  A 0 


[T]  = cos  A sin  A 
-sin  A cos  A 


[T]  ri> 


y m r°, 


In  order  to  transform  the  entire  state  vector,  define  a 6 x 6 transformation 


matrix: 


pr]  »r[T] 


Then  XL  - [<T]  XJ  (9) 

and  XI  - [<nTXL  . (10) 

It  is  assumed  that  [T]  is  available  from  the  IMU.  Alternatively, 
if  rate  gyros  are  used,  [T]  can  be  Integrated  from 

[T]  - [n][T]  ; [ T ( o ) ] - I (ID 


8 


where 


(12) 


This  last  method  of  computing  [T]  Is  included  as  a matter  of  interest. 

It  is  used  if  all  computation  is  in  the  LOS  frame  and  rate  gyros  are 
used.  In  our  formulation  A,  these  equations  are  incorporated  in  the 
state  equations,  and  in  formulation  B,  an  IMU  forms  [T ] . 

C.  Equations  of  Motion 

Look  first  at  the  deterministic  equations  of  motion. 

1.  Inertial  Frame 

In  inertial  coordinates,  the  equations  are  straightforward 
in  that  velocity  is  the  derivative  of  position,  and  acceleration  the 
derivative  of  velocity.  Furthermore,  acceleration  Is  modeled  as  a first 
order  Markov  process.  The  result  is  as  follows: 


- 

0 

0 

1 

0 

0 

0 

Dx 

0 

0 

0 

1 

0 

0 

Dy 

0 

0 

0 

0 

1 

0 

Vx 

0 

0 

0 

0 

0 

1 

vy 

0 

0 

0 

0 

-b 

0 

ax 

0 

0 

0 

0 

0 

-b 

a„ 

l 

L yi 

(13) 


or, 

n - [fi]xi 


(13a) 


2.  LOS  Frame 

There  are  at  least  two  approaches  to  deriving  the  LOS 
equations  of  motion,  one  of  which  is  by  the  consideration  of  the  cross 
product  which  arises  when  one  coordinate  system  is  rotating  relative 


to  mother.  A more  straightforward  approach  is  to  apply  the 
transformation  matrix,  [T],  to  equation  (13a).  The  result  is  as 
fol lows : 


or. 
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Ds 

0 

q 

1 

0 

0 

0 

Ds 

0 

D* 

-q 

0 

0 

1 

0 

0 

# 

V 

0 

0 

0 
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s 

Ve 

0 

0 

-q 

0 

0 

1 

Vi 

• 

as 

0 

0 

0 

0 

-b 

q 

as 

a* 

0 

0 

0 

0 

-q 

-b 

a z 

L . 

— 

L — 

XL 

= 

[FLjXL 

Note 

that 

the  equations 

of  motion  in  bo 

(14) 


(14a) 


and  so  long  as  the  Markov  correlation  time,  (1/b),  is  not  adapted,  the 
inertial  equations  of  motion  are  stationary.  However,  the  LOS  equations 
of  motion  are  not  stationary  because  fi  = q(t),  so  [FL]  = [FL(t)]. 

As  a result,  the  inertial  state  propagation  may  be  carried 
out  in  closed  form,  and  the  state  transition  matrix  is  constant.  How- 
ever, for  LOS,  the  state  must  be  integrated,  and  the  state  transition 
matrix  continually  computed. 

D.  State  Transition  Matrices 
1.  Inertial  Frame 

The  inertial  STM  follows  from  the  Laplace  transform  of 
equation  (13). 


[♦i]  -£ ' [ [si-CFur'j 


(15) 
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[+1] 


1 


0 

1 


0 0 
0 0 


t 

0 

1 

0 


0 b2  (e"bt>bt-l)  1 

5?(e’bt+bt-l ) 


1 


0 


1 


0 b (l-e’bt) 


0 


1 


1 


B(l-e~bt) 


o o n o 
o n o o 


-bt 


o 

-bt 


(16) 


where  t * At  * (t^+^ 
2 . LOS  F rame 


*k> 


(1?) 


In  principle,  [Jt]  could  be  derived  by  transforms,  given 
plenty  of  patience.  However,  It  can  be  derived  from  [*I]  as  follows: 


(18) 

but 

■ [%]  «4^*4  ■ [TklX 

(19) 

K4.l'  *--k+\  ' -Lk+1 

(19a) 

Substituting  (19)  and  (19a)  into  (18), 

■ r*')tVT?4 

(20) 

Hh,,.  ■ oWt.iu^fxL* 

(21) 

(«A*Lk 

(22) 

♦ 

.H)k  • [Tk„)tH)[Tk]T 

(23) 

This 

" an  exact"  state  transition  matrix  as  opposed  to  an 

"approximate 

state 

transition  matrix: 

» I ♦ [FL]at 

(23a) 
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The  former  is  exact  only  up  to  the  point  that  the  angular  rate,  5;, 
is  linearly  varying  over  the  time  interval  At.  The  latter  form  is 
used  in  a real-time  implementation  due  to  the  considerably  reduced 
computational  requirement. 

E.  State  Noise  and  Propagation  of  Variance 

While  the  deterministic  differential  equations  are  of  the 

form 

x = Fx 

The  stochastic  differential  equations  are  of  the  form 
x = Fx  + Gw 

where  w is  a white  noise  vector. 

1.  LOS  Formulation 


The 

stochastic  DE 

for 
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e estimate 
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Vs 
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0 

Vt 
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0 

-ft 
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“Vs 

0 
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0 

0 
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0 
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1 

0 
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0 
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0 

0 

-ft 

-b 

ap 

■ac 

0 

1 

— 

J6 

L S _ 

_ 

(24) 

(25) 


\! 


The  reason  for  the  6 term  is  as  follows: 

The  true  value  of  ft  is  unknown,  but  is  available  only  as  a measured 
quantity: 

“meet  ' "true  + B (27) 
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Note  that  this  is  not  a measurement  in  the  Kalman  filter  sense,  but  is  a 
measurement  nonetheless.  The  >s  and  \ { terms  are  those  random  jerque 
terms,  due  to  target  maneuverabi 1 i ty,  projected  onto  the  s and  ? axes. 


The  Q,  or  "noise11 , matrix  differs  for  each  of  the  three 
formulations . For  this  first  formulation. 


[Ql]  » E 


U 6s  6e] 


M 


(28) 


where  Q = variance  (t<) 


Q spectral  matrix  due  to  the  Markov  process. 

The  discrete  equivalent  of  the  continuous  [QL]  is 

.2  1 


[QLk]  ■ 


V1 


>LkAt 


(29) 


Note  that  the  spectral  term  builds  as  At  because  it  represents  a continuous 

2 

physical  process.  On  the  other  hand,  the  variance  term  builds  as  At 
due  to  the  fact  that  a rotation  rate  is  sampled,  and  held  constant 
computationally  for  At. 

Actually,  Q is  not  the  same  for  the  two  formulations  in 
which  LOS  propagation  of  variance  is  used.  For  the  first  formulation, 

Q.  reflects  the  error  in  the  rate  gyro  and  the  sampling  process.  For 

13 

the  second  formulation,  n is  derived  by  numerical  differencing  - normally 
a horrible  practice  in  a sensitive  computation,  but  less  so  in  this 
less  sensitive  propagation  of  variance  application. 

Propagation  of  variance  follows: 

[PLk]’  = [*Lk][PLk][*Lk]T  + [GLk][QLk][ULk]T  (30) 
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An  expanded  alternate  way  of  writing  this  equation  follows  if  we  define 


then 


2=  -Ds  vi-Vt“«sJT 

[PLk+1]*  = [<Mk][PLk3C<Mk]T  + g(Q6At)2T  + 


'4x4 


2x4 


_4x2 

V* 


(31) 

(32) 


2.  Inertial  Formulation 

The  system  matrix  [FI]  in  the  inertial  formulation  contains 
no  measured  quantities  as  did  [FL ] . 


(33) 
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Here,  y and  y are  the  target  random  jerque  terms  projected  onto  the  x 
x y 

and  y axes.  Then 
[QI]  » E 


r>  i 

[ Y Y ] 
x y 

X 

Y 

- 

L yj 

■ M 


(34) 


and 


[Q'V 


[QYl,  At^ 


(35) 


Propagation  of  variance  in  the  inertial  frame  then  follows: 


[PIk+1]'  - [♦I][PIk][*I]'  ♦ 


J4x4 


J4  x 2 


0, 


q: 


(36) 


"2x4  I vYlAt 

3.  Projection  of  the  Spectral  Matrix  onto  the  Computational  Frame 
It  is  a reasonable  assumption  that  the  acceleration  capability 
of  an  aircraft  normal  to  its  velocity  vector  is  greater  than  that  along 
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it  velocity  vector.  Suppose  that  the  Markov  spectral  matrix  is 
body  oriented: 


qu  is  the  spectral  term  along  the  target  velocity  vector,  while  qv  is  the  term 
normal  to  the  velocity  vector.  In  that  case,  q^  < qy.  It  can  be  shown 
that  the  following  is  the  rotation  from  [Q^]  from  aircraft  axes  to 
inertial  axes.  r 

I A O A O A A /A  A ft 


[Q  .]  » --V  (vV%+*y2%)  (38) 

ft  x + y ) (v  v ,q  -v  v q ) (v  q +v  q ) 

J x y^u  x yMu'  ' y Mu  x ^u' 

In  the  same  way,  [Qfa]  is  rotated  to  the  LOS  axes. 

<W,\> 

s P [ (VtVV^v)  {V{  °u+Vs  qv}  J 

F.  Measurements  and  Measurement  Noise 

While  no  measurements  are  simulated  as  such  in  the  error  analysis 
mode,  it  is  useful  to  look  at  the  Kalman  measurement  equation  transfor- 
mations. In  all  cases,  measurements  are  made  as  displacements  along 
the  tracker  axes. 


where 


YL  = [HL]  XL  + v 


v = measurement  error 
[HL]  = fl  0 0 0 0 0 


0 1 0 0 0 0 
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1.  Formulation  (A) 

*4*,  - tV*4  + t^k+13  (4i) 

Suppose  that  the  radar  range  variance  is  given  by  or?,  and 

p 

its  cross  range  angular  variance  is  given  by  <>  A-f.  If  the  measurement 
noise  covariance  matrix  is  given  by 

(42) 


[R]  = E[ v_  v.  ] = 


11 

R12 

21 

R22 

It  can  be  shown  that 
RL,,  = o^r 


+ (V 


(43) 


RLp  = RL^i  = 0 assuming  independence  between  range  and 


cross  range. 

2.  Formulation  (B) 

In  this  case,  for  the  sake  of  fast  and  accurate  propagation 
of  state,  this  operation  is  carried  out  in  inertial  coordinates,  while 
propagation  of  variance  takes  place  in  LOS  coordinates.  (44) 

= Onxi*  + 1^+1^  [k*-k+i^Ikk+i  " [HU[Tk+i]l>I]X4^ 

The  projected  inertial  state  estimate  is  transformed  to  form  the  LOS 

residual,  which  is  multiplied  by  a gain  [kL]  formed  by  LOS  terms.  The 

result  is  transformed  back  to  the  inertial  frame.  The  measurement 

noise  terms  are  similar  to  the  first  formulation,  but  the  cross  range 

angular  measurement  picks  up  an  additional  error:  the  error  in  the 

IMU  angular  position  measurement.  Suppose  that  the  variance  of  this 
2 

error  is  given  by  o A^. 
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Then  RL1 ^ = o r 


m-’*n 


2,2 

0 -r+0 

AT  aA 


RL11  = RL22  = 0 

3.  Formulation  (C) 

Here,  both  state  and  variance  are  propagated  in  inertial 
coordinate,  while  the  residual  is  still  in  LOS.  (46) 

■ [‘dfik  * CkIk+1][T]T  (yl^,  - CHL][Tk+1][ «]^ 

In  this  case,  the  gain  [kl]  is  formed  from  inertial  terms,  so  the  noise 
terms  in  equation  (45)  still  apply,  but  rotated  into  the  inertial  frame. 


2 2 

j-RI]  = (cos  A)r-|i  + (sin  A)r22 
(cos  A) (sin  A)(r11  - r?2) 


(cos  A)  (sin  A)(r-|-|  - r22) 

2 2 
(cos  A)r22  + (sin  A)^ 


where 


rll  " °r 


>2  + (D  )2(o2  +o2  .) 
r s'  aT  aA 


r,,  ■ r22  ■ 0 


It  should  be  noted  here  that  while  some  of  these  noise  terms  are 
getting  a bit  complicated,  the  filter  is  still  fully  coupled.  The 
truth  model  will  show  the  effects  of  approximations. 


G.  Kalman  Gain 
1 . LOS  Gain 

The  Kalman  gam,  given  by 


[kLk  + 1]  = [PLk  + 1]*[HL]T  ^[HL][PLkH]'[Hl]T*  [RL]^ 


-1 


(49) 


can  be  simplified,  considering  the  form  of  [HL] 


[kLk+]] 


2. 


[k!k+i] 


lpll'l 

pl12 

pl5i 

pl22 

p1?i 

p132 

jpl«l 

p^2 

pl51 

*52 

pi;, 

P^62 

a 1 Gain 

■ same 

way. 

p1ll 

pi  12 

Pi21 

pi22 

Pi31 

pi32 

pi41 

pi42 

pi  51 

pi52 

pl61 

Pi62 

(Pl^  + RLn) 

(pl?l  + RL21' 


(piJl  ♦ RIn) 


(pi 


21  + RI21) 


*p,12  + RL12) 

(Pi  22  + RL22^ 


H 


(50) 


(pi{1  + RI]2) 
(pi 22  * R 1 22 ^ 


-1 


(51) 


H.  A Posteriori  Covariance 

When  the  Kalman  gain  is  optimally  computed,  the  short  form 
covariance  nay  be  used. 

[Pk„]  ■ (I  - [k^KH])^,]  (5;) 


la 


This  form  will  also  be  used  in  the  sub-optimal  filters. 


However,  the  truth  model  uses  the  Joseph  form  to  compute  the 
true  covariance  resulting  from  a sub-optimal ly  formed  gain. 

[Pk+1]  - (I  - [kk+1][H])[Pk'+1](I  - [kk+1][H])T 


+ ^k.  . ■,  ] 


c 


V.  Means  of  Filter  Performance  Evaluation 


A.  Numerical  Values 

1.  Reference  Accuracy 

Formulation  A requires  a rate  gyro,  while  formulations  B 
and  C require  an  IMU.  The  values  chosen  for  rate  gyro  rate  error 
standard  deviation,  og,  and  IMU  angular  position  error  standard 
deviation,  o^,  were  chosen  to  yield  approximately  equal  estimation 
accuracy  for  the  decoupled  filters  when  the  target  trajectory  is 
parallel  to  an  axis  in  the  inertial  computational  frame. 

a = 5 mr./sec.  (54) 

P 

°£A  = 2 mr.  (55) 

All  secondary  reference  error  sources  were  neglected.  Specifically, 

IMU  drift  was  neglected  due  to  the  short  time  duration  of  individual 
anti-aircraft  encounters. 

2.  Measurement  Error 


Measurements,  in  the  Kalman  Filter  sense,  include  radar 


range  error,  aR,  and  cross  range  error,  a 

Nominal  values  were 

chosen  as 

aR  = 5m. 

(56) 

CAT  = 1 mr> 

(57) 

3.  Markov  Acceleration  Model  Parameters 

The  standard  deviation  in  target  acceleration  was  chosen  to 
? 

be  oa_  = 2 m/sec  , and  the  Markov  time  constant  to  be  t - 5 sec.  From 

a C 

this  follows  the  Markov  spectral  terms: 

qu  = 2<J  ac  (58) 
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(b9) 


q 3 4 q 
'u 

The  u and  v terms  correspond  to  the  target  vector  respectively.  The 
above  quantities  were  chosen  based  upon  previous  experience  where  a 
solutional  mode  Kalman  Filter  was  processing  actual  flight  data. 

B.  Target  Trajectories 

Even  to  the  extent  to  which  the  target  state  estimation 
problem  has  been  simplified  to  this  point,  it  is  difficult  to  draw 
conclusions  applicable  to  all  target  trajectories.  Since  a primary 
objective  has  been  to  examine  the  impact  of  decoupling  in  1 or  LOS 
coordinates,  two  trajectories  were  chosen:  both  of  which  are  a non- 
accelerating straight  line  fly-by,  but  one  is  parallel  to  an  axis  of 
the  fixed  computational  frame,  while  the  other  is  set  at  45°  to  the 
axis.  Both  are  shown  in  Figure  V-l. 

Test  Trajectories 


---  — > 

Zjweteri) 
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C.  Truth  Model 


Given  the  form  of  the  Kalman  Filter  estimation  equation, 

Vl  " Vk  + Kk+l^k+rHk+l*kV 


(60) 


If  the  optimal  Kalman  gain,  K^+1,  computed  according  to  the  equation 


Kk+1  = Pk+/  Hk+1  (Hk+l  PVl  Hk+1  + Rk+P 


1 


(61) 


When  all  of  the  Kalman  Filter  equations  precisely  describe  the  given 
problem,  then  the  a posteriori  covariance  is  given  by 

Pk+1  ^ " Kk+lHk+l^P’k+l  ‘ ^ 6 ^ 

In  practice,  if  the  gain  is  not  optimal  due  to  inadvertent  mismodeling 

or  due  to  purposeful  sub-optimization,  then  the  following  Joseph  form 

a posteriori  equation  reflects  the  measurement  incorporation  regardless 

of  how  K.  , was  obtained: 
k+1 

Pk+1  = (I  ~ Kk+lHk+PP’k+l(I  ' Kk+lHk+l^  + Kk+lRK+lKk-H 
This  last,  equation  is  the  essence  of  the  truth  model.  The  complete 
truth  model,  however,  incorporates  propagation  of  variance  according  to 
the  same  correct  dynamics  and  state  noise  error  sources  found  in  the 
sub-optimal  filter.  The  truth  model  concept  is  shown  below. 


Sub-optimal  Approximations 


I f 


Measurements 


Sub-optimal  Kalman  Filter 
(Using  equation  6?) 


Physically  Meaningless 

■ > 

Covariance  Matrix 


Sub-optimal  Gains 


Truth  Model 
(Using  equation  63) 


Physically  Meaningful 


Covariance  Matrix 


Figure  V-2 
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All  truth  model  computations  are  carried  out  in  fixed  cartesian  coordinates 
to  obtain  the  accuracy  of  an  exact  state  transition  matrix. 

D.  Error  Statistics  at  the  Target 

While  the  covariance  matrix  produced  by  the  truth  model  is 
correct  to  the  extent  that  the  various  model  parameters  are  realistic, 
it  does  not  directly  convey  the  information  of  interest,  which  is  the 
likelihood  of  hitting  a target.  What  is  required  is  the  position  error 
ellipse  at  the  target  which  applies  at  the  time  when  the  projectile 
reaches  the  target. 

1.  Projection  of  Covariance 

Following  the  measurement  at  time  t^,  the  truth  model 
covariance  matrix,  P. , exists.  It  is  most  conveniently  projected 
ahead  in  time  in  a fixed  cartesian  frame  as  follows: 

PPk  = [*I(TF)][Pk][-t>I(TF)]T  (64) 

The  closed  form  state  transition  matrix  is  given  for  any  time  interval, 
t,  by  equation  (16).  In  this  application,  the  time  to  be  substituted 
into  is  the  projectile  time  of  flight.  The  simple  time  of  flight 
expression  used  here  is 

RPk 

TFk  = B (65) 

where 

B = 1200  m/s 

C = 0.007  (m  sec)"1/2 

This  time  of  flight  is  a function  of  range  which  is,  of  course,  a 
function  of  time  of  flight.  Projected  range  follows  from 

» . [*l(TFk)]XI*  (66a) 

k 


0 


, A 

f 1 


f 
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RPk  - [DP2xk*  0P2,k]I/2 


(66b) 


Equations  (65)  and  (66)  are  solved  iteratively  to  produce  the  time  of 
flight  required  in  equation  (64).  DPXK  and  DPYK  in  (66b)  are  the  first 
two  terms  in  the  projected  inertial  state  vector,  XP^. 

2.  Error  Ellipse 

All  random  variables  within  the  Kalman  Filter  and  Truth  Model 
are  assumed  to  be  Gaussian.  In  two  dimensional  space,  the  curve  of  equal 
probability  density  is  an  ellipse,  given  by 

XPT  P_1XP  = c2  (67) 

Here  Xp  is  a two  vector,  X_P  = [DP  DP  ]T,  and  P is  the  2x2  position 

* y 

covariance  matrix  extracted  from  equation  (64). 

2 

The  constant  c follows  a chi-square  distribution  with  two 
degrees  of  freecom.  If  c = 1.180,  equation  (67)  describes  a parabola 
about  the  target  such  that  the  projectile  is  equally  likely  to  fall 
within  the  ellipse  as  it  is  to  fall  outside  of  the  ellipse.  The  ellipses 
shown  in  the  results  are  then  the  "50%"  ellipses. 

3.  Performance  Criteria 

The  error  ellipse  about  the  target  varies  as  a function  of 
time  for  any  given  filter  formulation.  It  is  desirable  to  reduce  all 
of  this  projected  data  to  a single  scalar  figure  of  merit  to  facilitate 
the  comparison  of  one  formulation  to  another.  In  fact,  two  are  used 
which  are  as  follows. 

a)  Sum  Square  Error 

If  PP^  is  the  projected  covariance  from  the  measurement 
at  time  k,  then  the  x direction  and  y direction  variances  are,  respectively 
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(68) 


o YK  = PPK 


The  sum  square  error  in  both  directions,  over  NM  measurements  is  then 

SSE  " E (o2XK  + j2Yk)  ( 7C 

k = 1 


the  mean  square  error  is  then 

, NM  ? 2 , 

MSE  = J.  Z (o^xK  + 0VK)  (71) 

m k = 1 

It  will  be  of  interest,  for  instance,  to  observe  successive  increments 
in  error  if  more  and  more  sub-optimal  approximations  are  incorporated 
into  a given  formulation. 

b)  RSS  Error  Ratio  (r.) 

Another  performance  criterion  relates  the  performance  of 
a particular  formulation  to  a theoretical  "best"  possible  formulation. 

Suppose,  as  before,  the  projected  position  various  are 

2 2 

’ xk  and  o y|(.  The  root  sum  square  error  is  then 

RSS  k = "/<i2xk  + c,2yk  ' (72) 

Furthermore,  suppose  that  the  RSS  position  error  of  the  "best"  filter 


at  the  same  time  is 


rss*k=  7<i2*xk+  °2*yk 


Then  the  average  ratio  of  these  RSS  errors  is 

1 NM  RSS*  K 


NM  K Tm  1 RSS  £ 


25 


Naturally,  since  any  practical  formulation  can  only  approach  the  ideal, 

RSS  > RSS*  and 

IN  K 

0 < 4 < 1 . (75) 

What  is  the  “best",  or  ideal,  filter?  For  our  purposes  it  will  be 
defined  as  a fully  coupled  filter  containing  no  approximate  computations, 
and  making  perfect  measurements.  The  sole  source  of  an  error  ellipse, 
or  RSSk*,  is  the  uncertainty  due  to  uncertain  target  maneuverability. 

The  ideal  filter  is,  in  this  case,  computed  in  fixed  inertial  coordinates 
due  to  the  availability  of  an  exact  state  transition  matrix. 

4.  Run  Sequence 

In  order  to  compute  the  above  performance  criteria,  the 
following  sequence  is  followed. 

a)  Establishment  of  Reference 

The  reference  consists  of  a file,  BREF,  containing  the  l 

• 1 

sequence  RSSK*,  1 <_  K <_  NM.  This  reference  is  unique  to  a given 
trajectory  and  target  model.  So  long  as  these  do  not  change,  the 
reference  file,  once  established,  need  not  be  changed. 
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RUN  SMAIN 

• Fully  Coupled  Formulation  C 

• Zero  Measurement  Error 


File  BTM 

•IC's  and  parameters  to  force  Truth  Model 
•Kalman  Gain  Sequence 


RUN  TMA1N 

• Truth  Model 

• True  Covariance  Projected  to 

Target 


b)  Sub-Optimal  Run  and  Evaluation 

To  perform  this  sequence,  program  SMAIN  simulates  the 
sub-optimal  filter,  then  TMAIN  computes  the  true  covariance  and  then 
the  performance  criteria. 


RUN  SMAIN 

•Formulation  A,  B,  or  C 
• Any  Combination  of  Sub-Optimization: 


File  Write 


File  BTM 

•IC's  and  parameters  to  force  Truth 
Model 

•Kalman  (lain  Sequence 


RUN  TMAIN 
•Truth  Model 

•True  Covariance  Projected  to  Target 
•Performance  Criteria  Computed 


Fi  le  Write 


File  BREF 

. RSS*K,  1 < K < MM 


File  BPLT 

•Variables,  Covariances  Recorded  for 
Subsequent  Plots,  Ellipses 


VI.  Results 


A.  Sub-Optimization  Sequence 

For  each  of  the  three  formulations  considered,  it  is  possible 
to  start  with  a "perfect"  filter,  yielding  estimation  error  due  only 
to  target  maneuverability,  then  to  see  the  amount  of  error  added  at 
each  step  along  the  way  to  a real-time  implemented  sub-optimal  filter. 

1.  Formulation  (A) 

In  formulation  A,  both  state  and  variance  are  propagated 
in  LOS  coordinates.  The  sub-optimization  steps  are  as  follows: 

• "perfect",  or  reference  filter 

• Introduction  of  rate  gyro  error,  o(, 

• Approximation  of  state  transition  matrix  by  first  two 
terms  of  a Taylor  series 

*L  ■ I + FLAt  (76) 

• Decoupling  of  covariance.  In  order  to  accomplish  a real- 
time solution,  decoupling  is  required  where  covariances 
between  position,  velocity,  and  acceleration  in  the  s 
direction  and  those  in  the  1 direction  are  assumed  to  be 
zero.  Decoupling  in  this  line  of  sight  axis  will  be 
abbreviated  LOS  DEC. 

2.  Formulation  (B) 

In  this  formulation,  propagation  of  state  takes  place  in 
inertial  (I)  coordinates,  and  propagation  of  variance  in  LOS  coordinates. 
The  sub-optimization  steps  are  as  follows: 

• "perfect",  or  reference  filter 

• Introduction  of  IMU  angular  measurement  error, 

• Numerical  differencing  of  IMU  position  to  produce  rates 
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required  for  state  transition  matrix 
Approximate  STM: 


*L  = I + FLAt 

• Decoupling  of  covariance  matrix  in  LOS  coordinates  (LOS  DEC) 
3.  Formulation  (C) 

Here,  both  state  and  variance  are  propaged  in  inertial 
coordinates.  Recall  that  the  state  transition  matrix  may  use  written 
in  closed  form,  without  approximation.  The  sub-optimization  stops  are 
as  follows: 

. “perfect",  or  reference  filter 

• Introduction  of  IMU  angular  measurement  error 

• Decoupling  of  covariance  matrix  in  inertial  (I)  coordinates 
(I  DEC) 

B.  Fully  Coupled  Sensitivities  to  Measurement  Error 

The  conditions  under  which  the  following  sensitivities  were 
computed  are  as  follows: 

• Trajectory  #1  (10  second  fly-by  at  300  m/s,  500  m range 
at  cross-over) 

• 8 Hz  measurement  rate 

• Single  propagation  of  state  and  variance  in  between  each 
measurement 

• All  previously  outlined  sub-optimization  steps  are  in  effect 
with  the  exception  of  decoupling. 

Referring  to  Figures  VI-1,  VI-2,  and  VI-3,  the  following 
observations  may  be  made 

• The  percent  increase  in  mean  square  error  takes  as  a 
baseline  the  reference  filter. 
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Apparent  in  formulation  A and  B is  a lower  limit  of  2.6% 
below  which  the  error  increase  will  not  fall  as  measurement 
error  approaches  zero.  This  error  is  due  to  the  first  order 


^ = I + FLAt  approximation. 

• No  such  limit  is  observed  for  formulation  C where  an  exact 
state  transition  matrix  is  available. 

• The  limit  in  A and  B is  not  a fundamental  limitation,  but  is  a 
function  of  the  order  of  the  state  transition  matrix  (STM) 
approximation  and  the  time  interval  over  which  the  approximate 
STM  is  effective.  In  other  words,  there  is  considerable 
flexibility  in  modifying  this  limit. 

• Note  the  approximately  equivalent  accuracy  for  a.  = 5 mr/s 

P 

in  A and  = 2 mr  in  B.  These  numbers  will  form  the  baseline 
for  comparisons  to  follow. 


I \ 
1 1 


\ j 
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C.  Added  Error  In  Real  Filters 


Referring  to  Figure  VI-4,  it  may  be  seen  the  error  contributions 
of  each  of  several  steps  from  the  perfect  measurement  reference  filter 
to  the  real-time  decoupled  filter.  The  following  observations  will 
help  in  interpreting  the  figure. 

* The  baseline  error  is  due  only  to  the  target  maneuver  uncertainty. 

, Rate  gyro  and  1MU  errors  were  chosen  to  provide  approximately 

equivalent  performance  in  formulations  A and  C,  trajectory  #1. 

* These  results  are  not  general.  They  are  to  some  extent  trajectory 
dependent. 

• Recall  that  trajectory  #1  is  parallel  to  an  axis  of  the  inertial 
computational  frame,  while  trajectory  #2  is  at  45°  to  the  axes. 
When  variance  is  propagated  in  LOS  coordinates,  as  in  A and  B, 
there  is  naturally  no  difference  in  results  in  comparing  these 
two  trajectories. 

• When  variance  is  propagated  in  the  inertial  frame,  a greater 
penalty  is  paid  due  to  decoupling  when  the  target  is  flying  at 
45°  to  the  axes.  More  will  be  said  about  decoupling  in  section 
VI. D. 

• The  error  contribution  due  to  the  approximate  computation  of  the 
LOS  STM  can  be  reduced  by  a higher  order  approximation  or 
multiple  propagation  steps  in  between  measurements. 

* The  error  due  to  numerical  differencing  in  B required  to  form 
angular  rates  does  not  appear  in  the  figure.  The  derived 
angular  rate  is  needed  only  in  producing  some  STM  terms  for 
propagation  of  variance,  because  state  is  propagated  in  inertial 
coordinates.  The  use  of  the  derived  rates  is  in  such  an 
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uncritical  computation  that  the  added  error  is  only  0.03X.  An 
analytical  solution  for  this  last  figure  is  difficult.  Rather, 
a random  number  generator  corrupted  IMU  angular  position 
measurement  before  deriving  rates.  This  result  is  then  the 
result  of  a Monte  Carlo  simulation. 

D.  Computation  of  Kalman  Gain:  Comparison  of  LOS  Coordinates 
Versus  Inertial  Coordinates 

Figure  VI-4,  in  the  previous  section,  showed  a marked  difference 
in  added  error  when  decoupling  is  performed  in  LOS  coordinates  as 
compared  to  inertial  coordinates.  In  order  to  understand  the  reasons 
for  this  difference,  it  is  probably  most  easily  done  by  examining 
graphical  results. 

1 . Posi tion  Variance 

Figure  VI-5  shows  for  trajectory  *1  the  variance  in  LOS 
2 2 

range  (on  ),  cross  ranqe  (on  ),  and  the  covariance  (on  n ).  The 

L)s  Uc  L>s»Ut 

trajectory  period  is  10  seconds,  and  the  filter  transient  is  substantially 

complete  in  two  seconds.  Figure  VI-6  shows  the  corresponding  quantities 

in  inertial  (I)  coordinates.  Recall  from  Figure  V-l  that  trajectory  *1 

consists  of  of  a straight  line  fly-by  parallel  to  the  inertial  x axis, 

crossing  the  y axis  at  a range  of  500m  at  5 seconds.  The  I position 

2 2 

variances  are  then  o and  o while  the  covariance  is  o . These 

x y x ,y 

plots  are  often  taken  from  terms  of  the  LOS  and  I covariance  matrices, 
respectively,  when  the  filters  are  not  yet  decoupled. 

The  variances  exhibit  the  typical  Kalman  Filter  pattern: 
growth  during  intermeasurement  periods,  and  abrupt  drops  at  measurement 
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Percent  Increase  in  Mean  Square  Error  Due  to  Sub-Optimization 


Figure  VI-4 


times.  Before  decoupling,  the  sum  square  uncertainty  is  much  the 
same  for  LOS  and  I : 

(o2d  (t)  + o2d  (t))  ~(o2d  (t)  + o2d  (t))  (78) 

s ‘ x y 

the  notable  difference  is  that  in  general,  the  LOS  covariance  is  much 
less  than  the  I counterpart. 


0, 


’Dx,Dy' 


(79) 


A small  covariance  indicates  that  the  error  ellipse  is  closely  aligned 
to  the  coordinate  axes.  Figure  VI-7,8,9,10,  11  show  a time  sequence  of 
LOS  position  error  ellipses  demonstrating  the  near  alignment  of  the 
position  ellipses  with  the  LOS  axes.  While  LOS  ellipses  were  plotted, 

I ellipses  could  have  been.  Prior  to  decoupling,  they  look  essentially 
the  same. 


The  reason  for  this  alignment  is  as  follows.  The  error 
ellipses  have  a natural  tendency  to  grow  parallel  to  the  aircraft  axes 
due  to  the  aircraft's  greater  acceleration  uncertainty  normal  to  its 
velocity  vector  in  comparison  to  the  uncertainty  along  the  velocity 
vector.  However,  the  measurements  made  by  the  radar  are  made  in 
range  (along  the  s axis)  and  cross  range  (along  the  * axis).  Due  to 
this  measurement  geometry,  the  a posterior  position  error  ellipses 
are  continually  realigned  to  the  tracker  (LOS)  axes.  The  implications 
of  this  alignment  will  become  apparent  in  the  following  sections. 

Now,  consider  plots  for  the  same  quantities  for  trajectory 
#2.  Figure  V-l  shows  that  this  trajectory  makes  a 45°  angle  with  the  I 
axes.  First  of  all,  the  plots  of  op  , op  , op  ,qare  identically  the 
same  for  T2  as  they  were  for  T1 . The  rotating  LOS  coordinates  have 
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no  absolute  inertial  reference,  so  T1  and  T2  look  the  same  in  the 


LOS  frame.  However,  Figure  VI-12  shows  a considerable  change 

p 2 

in  o D , a D , and  aD  D for  T2  as  compared  to  T1  in  Figure  VI-6, 
x y x*  y 

Figures  VI-13,14,15,16,17  shows  the  sequence  of  LOS 
(essentially  the  same  as  I)  position  ellipses  for  trajectory  #2.  As 
was  the  case  for  Tl,  the  T2  ellipses  are  substantially  aligned  to 
the  tracker  axes. 

2.  Kalman  Gains  of  Fully  Coupled  Filters 

When  the  matrix  inversion  is  carried  out  in  the  Kalman  Gain 
equation  (equation  50,51).  The  gain  is  as  follows. 


K = 


1 


c < P'n +rn  > ( P22*r  22 ) - l'2*r  1 2 ! 


pll 

p12 

p21 

p22 

P31 

p32 

CL 

p42 

P51 

p52 

P61 

P62 

< p22+r22 ) ■ ^p12+r12^ 
-(PiVri2)  (p,',*rn> 


(80) 


The  LOS  position  gains  are  interpreted  as  follows: 

A 

(Modification  of  Ds)  = (Kj  grange  residual) 

(Modification  of  Dt)  = (K  ^ ^) (range  residual) 

(Modification  of  Ds)  = (K-j  2)  (cross  range  residual) 
(Modification  of  D^)  = (k2  2)(cross  ran9e  residual) 

Figure  VI-18  shows  plots  of  the  LOS  position  gains  for  trajectory  #1. 
Although  range  and  cross  range  measurements  are  essentially  simultaneous, 

suppose  the  measurement  in  range  precedes  that  of  cross  range.  First, 

A 1/ 

the  range  measurement  decreases  Os  uncertainty  via  1,1.  From  the 
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figure,  the  decrease  in  Dv  due  to  range  measurement  is  negligible 
because  ^ is  small.  However,  at  crossover,  the  cross  range  accuracy 
increases  (see  equation  43),  so  the  ^ term  is  significant  in  further 

A ’ A 

reducing  P$  uncertainty.  Finally,  cross  range  reduces  D i via  «2  2 

It  is  interesting  to  look  at  the  K-j  ^ term.  From  equation 
(80),  with  r1  2 = 0, 

(81) 


rllP12 


1,2 


[(P1'1+^11)(P2Vr22^  " p122] 


While  p.,  = a , is  always  small.  Figure  VI-5  shows  that  it  reaches  its 
maximum  at  crossover.  Simultaneously,  cross  range  measurement  error,  r^, 
reaches  a minimum  at  crossover.  The  net  result  is  the  significant  K -j  ^ 
at  crossover.  The  reason  for  the  interest  in  ^ 1S  that  it  is  the  most 
important  of  the  terms  lost  when  the  filter  is  decoupled. 

A similar  line  of  reasoning  can  be  applied  to  the  position 
gains  in  inertial  coordinates.  Figure  VI-19  shows  that  in  this  case, 
both  cross  terms  0 andK^  ^ are  significant.  Both  will  be  lost  to 
decoup 1 ing. 

Now,  consider  trajectory  #2  in  comparison  with  trajectory  #1. 
For  the  reason  tha  the  LOS  covariance  terms  are  identical  for  T1  and  T2, 
the  Kalman  gains  are  also  the  same  for  T1  and  T2.  However,  the  inertial 
position  gains  for  T2,  shown  in  Figure  VI-20  are  substantially  different 
than  those  for  T1  in  Figure  VI-19.  Again  there  are  significant,  but 
different  cross  terms  K-j  ^ an(l  K2  | decoupling.  The 

implications  at  this  point  are  as  follows: 

• LOS  position  gains  are,  to  saome  extent,  trajectory 

Independent 


• The  trajectory  dependence  of  inertial  gain  cross  terms 
suggests  that  upon  their  loss  in  decoupling,  the  error 
introduced  will  be  greater  for  some  trajectories  than  for 
others. 

So  far,  only  the  four  position  gains  have  been  discussed. 
There  are  also  velocity  gains  which  correct  velocity  estimates  based 
upon  position  measurements,  and  acceleration  gains  which  correct 
acceleration  estimates  based  upon  position  measurements.  The  effects 
apparent  in  the  position  gains  are  also  present,  to  a somewhat  lesser 
extent,  in  the  velocity  and  acceleration  gains.  The  velocity  and 
acceleration  gain  counterparts  of  the  previous  position  gain  plots  are 
included  and  summarized  as  follows: 


Figure 

Title 

VI-21 

T1 

& T2 

LOS 

Velocity  Gains 

VI-22 

TL 

I 

Velocity  Gains 

VI-23 

T2 

I 

Velocity  Gains 

VI-24 

T1 

A T2 

LOS  Acceleration  Gains 

VI-25 

T1 

I 

Acceleration  Gains 

VI-26 

T2 

I 

Acceleration  Gains 

3.  Kalman  Gains  of  Decoupled  Filters 

Decoupling  is  performed  to  reduce  computation  in  order  to 
permit  real-time  gain  computation.  In  the  three  dimensional  solution, 
a fully  coupled  filter  has  45  different  terms,  while  a decoupled  filter 
has  only  18. 
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Decoupl ing  assumes  statistical  independence  in  uncertainties 
from  one  computational  axis  to  another.  That  is,  0,  y = 0, 

etc.  The  elimination  of  covariance  terms  implies  that  error  ellipses  are 
parallel  to  coputational  axes.  As  it  has  been  seen,  the  position  error 
ellipses  are  nearly  aliened  to  the  LOS  axes,  so  that  it  may  be  concluded 
that  the  pragmatic  decoupling  decision  is  more  in  keeping  with  the  physical 
reality  of  the  LOS  frame  than  the  1 frame.  It  is  for  this  reason  that 
Figure  VI-4  shows  greater  error  introduced  by  1 decoupling  than  by  LOS 
decoupling. 

When  the  target  is  flying  parallel  to  the  I axes,  its 
acceleration  uncertainty  tends  to  rotate  the  error  ellipses  away  from  the 
tracker  axes  toward  the  I axes  i nbetween  measurement  times.  This  is 
relatively  favorable  to  I decoupling.  However,  when  the  trajectory  is 
at  45°  to  the  I axes,  the  resulting  growth  in  covariance  cross  terns  is 
relatively  unfavorable  to  1 decoupling.  Hence,  the  difference  in  T1  and 
T2  decoupling  error  in  formulation  C,  Figure  V 1 -4 . Again,  the  LOS  gains 
are  the  same  for  T1  and  T2,  so  the  error  introduced  by  decoupling  is  the 
same  for  both. 

When  the  state  vectors  are  composed  as  they  are  in  this  report, 
decoupling  result  in  the  zeroing  of  covariance  matrix  terms  whose  row 
plus  column  sum  is  odd.  This  causes  the  square  matrix  in  equation  (80) 
to  become  diagonal.  All  of  the  Kalman  Gain  cross  terms  are  then  eliminated, 
leaving  only  *3J,  \v  K5>,,  and 

When  LOS  is  decoupled  as  shown  in  Figure  VI-27,  only  ^ 
and  K,j  ? remain  of  the  position  gains.  Note  that  in  comparison  with 
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the  undecoupled  gains  in  Figure  VI-18,  the  important  ^ ,,  term  is 
lost,  hut  Kj  -j  and  are  substantially  unchanged.  However,  Figure 
VI-28  shows  the  1 decoupled  gains  where  K,  0 and  K are  lost,  but 
in  comparison  with  Figure  VI-19,  the  remaining  1 and  „ have  been 
considerably  changed.  In  addition.  Figure  VI-29  shows  that  T2  I decoupled 
gains  differ  from  T1  I decoupled  gains.  The  implications  of  these 
results  are  as  follows: 

• For  both  I and  LOS,  information  is  lost  in  the  loss  of  the 
K1  ^ term,  with  the  inevitable  loss  in  estimation  accuracy. 

• Since  1 and  0 remain  essentially  unchanged  in  decupling 
LOS,  the  possibility  exists  that  the  lost  Kj  ^ could  be 
approximated  through  equation  (81)  by  use  of  an  empirical 
relation  for  the  uncomputed  p^.  The  generation  of  empirical 
relations  appears  feasible  due  to  the  trajectory  independent 
nature  of  the  LOS  covariance  matrix,  and  due  to  the  relatively 
minor  changes  in  that  covariance  matrix  due  to  decoupling. 

• For  just  the  opposite  reasons,  the  generation  of  approximate 
gain  terms  lost  in  I decoupling  would  appear  to  be  quite 
difficult. 

Again,  the  results  in  the  velocity  and  acceleration  gains  are 
similar  to  those  seen  in  the  position  gains.  A summary  of  the  graphical 
results  is  as  follows: 

Title 

T2  LOS  Decoupled  Velocity  Gains 

I Decoupled  Velocity  Gains 

I Decoupled  Velocity  Gains 


Figure 

V 1-30 

T1 

VI-31 

T1 

VI-32 

T2 

VI-33 

T1 
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Figure 

Title 

VI-34 

T1 

I Decoupled  Acceleration  Gains 

VI-35 

T2 

I Decoupled  Acceleration  Gains 

4.  Projected  Position  Error  Ellipses 

While  the  computed  Kalman  Filter  position,  velocity,  and 
acceleration  uncertainty  all  have  a bearing  on  the  ultimate  probability 
of  hitting  the  target,  the  projected  position  ellipse  conveys  the  most 
information  in  this  regard.  As  described  in  Chapter  V,  a projected 
ellipse  is  constructed  such  that  the  projectile  is  equally  likely  to 
land  inside  or  outside  the  ellipse. 

Figure  VI-36,37,38,39,40  show  the  sequence  of  projected 
position  ellipses  resulting  from  the  decoupled  LOS  filter  acting  on 
trajectory  #2.  The  ellipse  is  relatively  large  at  T = 2 because  the 
filter  is  just  finishing  its  transient.  By  the  time  of  cross  over, 

T = 5,  the  ellipse  is  quite  small  due  to  the  high  quality  cross  range 
measurement  due  to  short  range  to  the  target.  Finally,  by  T = 10,  the 
ellipse  is  again  large,  partially  due  to  the  degrading  cross  range 
accuracy,  but  primarily  due  to  the  long  flight  time  as  the  projectile 
is  now  catching  up  to  a departing  target. 

Finally,  Figure  V 1-41  ,42,43,44,45  show  the  same  sequence 
resulting  from  the  I decoupled  filter.  The  line  of  sight  axes  are 
shown  on  the  figures,  but  are  not  used  computationally.  Note  the 
generally  larger  ellipses  in  this  sequence  relative  to  the  last  sequence, 
showing  the  poorer  accuracy  due  to  decoupling  in  inertial  coordinates. 
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T 1 INERTIAL  POSITION  COUARIANCE 


FIGURE  UI 


=2  LOS  POSITION  ELLIPSE 


FIGURE  UI 


T 1 T s6  LOS  POSITION  ELLIPSE 
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T2  INEPT  I AL  POSITION  COUAPIhNCE 
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T2  T=8  LOS  POSITION  ELLIPSE 


FIGURE  01 


T 1 INERTIAL  VELOCITY  GAINS 


T2  I HE 


T 1 INERTIAL  DEC  POSITION  GAINS 
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T 1 INERTIAL  DEC  ACCELERATION  GAINS 
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T2  T=2  LOS  DEC  PROJECTED  POSITION 
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T2  1-2  INERTIAL  DEC  PROJECTED  POSITION 
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E.  Results  Not  Obtained 


As  the  result  of  careful  computer  program  development  with 
numerous  consistency  checks,  it  is  believed  that  the  results  presented 
are  accurate.  However,  the  danger  exists  that  general  conclusions 
may  be  drawn  from  limited  data.  The  further  work  that  should  be 
accomplished  to  generalize  the  conclusions  is  as  follows: 

F.  Trajectories 

A wider  range  of  target  trajectories  is  desirable,  including 
straight  lines  at  various  ranges  and  velocities,  and  accelerating 
trajectories. 

G.  Computation  Rates 

Measurement  incorporation  rates  of  other  than  8 Hz  should  be 
considered.  Multiple  propagation  of  variance  stops  in  between  measurements 
might  aid  in  reducing  the  error  due  to  approximate  LOS  propagation  of 
variance.  To  this  same  end,  a higher  order  approximation  to  4^  should 
be  investigated. 

H.  Error  Sources 

The  problem  should  be  carefully  reexamined  to  assure  that  all 
significant  error  sources  have  been  adequately  modeled. 


I.  A Fourth  Formulation 

This  study  has  included  rate  gyro/LOS,  IMU/LOS,  and  IMU/I. 
To  round  out  the  considerations,  especially  for  new  system  design, 
rate  gyro/ I should  be  investigated. 
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j.  Improvement  on  LOS  Decoupling 


The  results  obtained  with  respect  to  LOS  decoupling  suggest  the 
possibility  of  the  recovery  of  some  of  the  lost  decoupling  accuracy  with 
a relatively  minor  computational  cost.  This  possibility  should  be  pursued. 
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model  are,  respectively,  SMAIN  and  TMHIN.  These  programs  call  a sequence 
of  subroutines,  most  of  which  are  used  by  both.  With  a familiarity 
with  the  mathematical  development  in  Chapter  IV,  along  with  the  program 
comments,  the  programs  should  be  easily  followed. 
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100  * R.  DEMOYER  1978 

110  * SUB-OPTIMAL  MAIN  PROGRAM 

120  CGMMGN/T IME/T/ DT»  TMX<  NT, TF 

1 30  CQMMON/SUB/KSUB  < 20 ) , KSO,  K TRANS 

140  * TEST  PRINT  FILE  # 1 

150  OPENFILE  1.  "TPl" 

160  REWIND  1 

170  ENDFILE  1 

180  * TEST  PRINT  FILE  #2 

190  OPENFILE  2, "TP2" 

200  REWIND  2 

210  ENDFILE  2 

220  * TEST  PRINT  FILE  #3 

230  OPENFILE  3,  " TPS" 

240  REWIND  3 
250  ENDFILE  3 

260  * KALMAN  GAIN  FILE  TO  FORCE  TRUTH  MODEL 
270  OPENFILE  4,  " BTM",  "NUMERIC" 

280  REWIND  4 
290  ENDFILE  4 
300  * PLOT  FILE 

310  OPENFILE  5,  "BPLT ">  "NUMERIC" 

320  REWIND  5 
330  ENDFILE  5 

340  * OPTIMAL  REFERENCE  FILE 
350  OPENFILE  6,  "REFF",  "NUMERIC" 

360  REWIND  6 
370  LIBRARY  "SICS" 

380  LIBRARY  "TTARG" 

390  LIBRARY  "TPTRAN" 

400  LIBRARY  " TSTM" 

410  LIBRARY  "TO" 

420  LIBRARY  "TPROP" 

430  LIBRARY  " TRMAT " 

440  LIBRARY  "SKGAIN" 

450  LIBRARY  "TPOST" 

460  LIBRARY  " TPLOT" 

470  LIBRARY  "SPROJ" 

480  LIBRARY  " SUBOP " 

490  * KF0RM=1  RATE  GYRO,  LOS  FRAME 
500  * KF0RM=2  DERIVED  RATES,  LOS  FRAME 
510  * KF0RM=3  I MU,  FIXED  INERTIAL  FRAME 
520  * INITIAL  CONDITONS 
530  CALL  SICS<KFORM> 

540  * INITIAL  WRITE  TO  PLOT  FILE 

550  CALL  TPLOT 

560  * NT  FILTER  STEPS 

570  DO  1 N=1 , NT 

580  * COMPUTE  STATE  TRANSITION  MATRIX 
590  CALL  TSTM<KFORM) 

600  * COMPUTE  STATE  NOISE  MATRIX 
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610 

620 

630 

640 

650 

660 

670 

680 

690 

700 

710 

720 

730 

740 

750 

760 

770 

780 

790 

800 

810 

820 

830 

840 


CALL  TQ< KFORM) 

T =N*DT 

* PROPAGATION  OF  VARIANCE 
CALL  7 PROP  < KFORM ) 

* A PRIORI  VALUES  TO  PLOT  FILE 
CALL  TPLOT 

* COMPUTE  MEASUREMENT  NOISE  MATRIX 
CALL  TRMAT< KFORM) 

* COMPUTE  KALMAN  GAIN 
CALL  SKGAINI KFORM) 

* COMPUTE  A POSTERIORI  COVARIANCE 
CALL  TPOST< KFORM) 

* PROJECT  VARIANCE  TO  TIME  OF  IMPACT 
CALL  SPROJ 

* A POSTERIORI  VALUES  TO  PLOT  FILE 
CALL  TPLOT 


1 CONTINUE 
ENDFILE  1 
ENDFILE  2 
ENDFILE  3 
ENDFILE 
ENDFILE 
IF  < KSUB( 1 ) 
END 


4 

5 


EG  1)  ENDFILE  6 
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10O  * R DEMOYER  1978 

110  * TRUTH  MODEL 

120  COMMON  HI  ME'  T,  DT,  TMX,  NT,  TF 

130  * TEST  PRINT  FILE  «1 

140  OPENFILE  1, "TP1" 

150  REWIND  1 

160  ENDFILE  1 

170  * TEST  PRINT  FILE  #2 

180  OPENFILE  2,  ,,TP2,• 

190  REWIND  2 

200  ENDFILE  2 

210  * TEST  PRINT  FILE  #3 

220  OPENFILE  3,  "TPS" 

230  REWIND  3 
240  ENDFILE  3 

250  * KALMAN  GAIN  FILE  FROM  SUB-OPTIMAL  RUN 
260  OPENFILE  4,  "BTM",  "NUMERIC" 

270  REWIND  4 
280  * PLOT  FILE 

290  OPENFILE  5,  "BPL T",  "NUMERIC" 

300  REWIND  5 
310  ENDFILE  5 

320  * OPTIMAL  REFERENCE  FILE 
330  OPENFILE  6,  "REFF",  "NUMERIC" 

340  REWIND  6 
350  LIBRARY  "TICS" 

360  LIBRARY  "TTARG" 

370  LIBRARY  " TPTRAN" 

380  LIBRARY  "TSTM" 

390  LIBRARY  "TO" 

400  LIBRARY  "TPROP" 

410  LIBRARY  "TRMAT " 

420  LIBRARY  "TKGAIN" 

430  LIBRARY  "TPOST" 

440  LIBRARY  "TPLOT" 

450  LIBRARY  "TPROJ" 

460  * KFORM  = 1 RATE  GYRO,  LOS  FRAME 
470  * KFORM  » 2 DERIVED  RATES,  LOS  FRAME 
480  * KFORM  =*  3 I MU,  FIXED  INERTIAL  FRAME 
490  # I NIT I LA  CONDITIONS 
500  CALL  TICS (KFORM) 

510  * INITIAL  WRITE  TO  PLOT  FILE 

520  CALL  TPLOT 

530  * NT  FILTER  STEPS 

540  DO  1 N*1 , NT 

550  * COMPUTE  STATE  TRANSITION  MATRIX 
560  CALL  TSTM (KFORM) 

570  * COMPUTE  STATE  NOISE  MATRIX 
580  CALL  TO (KFORM) 

390  T»N*DT 

600  * PROPAGATION  OF  VARIANCE 
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610  CALL  TPROP< KFORM) 

620  * A PRIORI  VALUES  TO  PLOT  FILE 
630  CALL  TPLOT 

640  * COMPUTE  MEAUREMENT  NOISE  MATRIX 
650  CALL  TRMAT ( KFORM ) 

660  • COMPUTE  KALMAN  GAIN 
670  CALL  TKGAIN< KFORM) 

680  * COMPUTE  A POSTERIORI  COVARIANCE 
690  CALL  TP0ST1 KFORM) 

700  * PROJECT  VARIANCE  TO  TIME  OF  IMPACT 
710  CALL  TPROJ 

720  * A POSTERIORI  VALUES  TO  PLOT  FILE 
730  CALL  TPLOT 

740  * COMPUTE  SUM  SQUARE  ERROR 
750  CALL  TEVAL  < 1 ) 

760  1 CONTINUE 

770  • PRINT  SUM  SQUARE  ERROR,  FIGURE  OF  MERIT 
780  CALL  TEVAL  < 2 ) 

790  ENDFILE  5 
800  END 
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100  * R DEMOYER  197S 
110  * INITIAL  CONDITIONS 
120  SUBROUTINE  SICS (KFORM) 

130  COMMON/ TST/ XT I <6>,  XTL<6> 

140  COMMON/ TRAJ/ X TRAJ <2, 3) 

150  COMMON /'NPAR/ NS,  NM,  ND 

16.0  COMMON/ T IME / T, DT, TMX. NT, TF 

170  COMMON/ COV/PT I (6#  6).  PTL<6>,  6) 

180  COMMON/ TRANS /A, AD,  TM<2, 2),  TMF  < 6, 6) 

190  COMMON/ RPAR/S I GR,  SIGAZ,  SI GANG,  R<2,  2) 

200  COMMON /QPAR/SIGAC.  TCAC , QBET < 2 ) 

210  COMMON/ TEST /KT 

220  COMMON  /’SUB/  KSUB  (20) » KSO.  K TRANS 

230  DIMENSION  KSB<20) 

240  SAVE  ALL 

250  DATA  KSUB/ 20*0/ 

260  * KFORM  = FORMULATION  #1,2,  OR  3 
270  * KT  = TEST  PRINT  FILE  NUMBER 1 , 2, OR  3 
280  * TMX  = FINAL  TIME 
290  * NT  = # FILTER  STEPS 

300  * TRA.J  = KTR  * TRAJECTORY  # 1,2,  OR  3 
310  PRINT,  "KFORM,  KT,  TMX,  NT,  TRAJ" 

320  READ,  KFORM.  KT,  TMX,  NT,  KTR 

330  « SEE  SUBROUTINE  SUBOP  FOR  SUB-OPTIMAL  CODES 
340  PRINT, "#.  SUB  OPTS" 

350  READ. NSUB,  <KSB< I >,  1 = 1,  NSUB) 

360  * KSUB < 1 ) = 1 TO  WRITE  REFERENCE  FILE 
370  DO  30  1=1, NSUB 
380  DO  30  J= 1 , 20 

390  30  IF  < J EQ  KSB(I))  KSUB(J)  = 1 
400  KS0=0 

410  * NOMINAL  TIME  BASE 

420  T =0 

430  DT=TMX -NT 

440  * TRAJECTORY 

450  GO  TO  (110,  120,  130,  140,  150,  160),  KTR 

460  * TRAJECTORY  # 1 

470  * X POSITION 

480  110  CONTINUE 

4°0  XTRAJ< 1 • 1 ) =-1500 

500  * X VELOCITY 

510  XTRAJO  2)  =300 

520  * Y POSITION 

530  X TRAJ <2,  1 >=500 

540  GO  TO  300 

550  * TRAJECTORY  # 2 

560  120  CONTINUE 

570  S2=SQRT<2  ) 

580  XTRAJ< 1,  1 )=-1000*S2 
590  X TRAJ  < 1 . 2 > = 1 50*S2 
600  XTRAJI2,  1 >=-500*S2 
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610  XTRAJ12,  2)=150*S2 

620  GO  TO  300 

630  130  GO  TO  300 

640  140  GO  TO  300 

650  150  GO  TO  300 

660  160  GO  TO  300 

670  300  CONTINUE 

680  * 2 DIMENSIONAL  MODEL 

690  * ND  * « DIMENSIONS 

700  ND-2 

710  * NM  = # MEASUREMENTS  < RANGE  & CROSS  RANGE) 
720  NM=2 

730  * NS  = # STATES 
740  NS=ND«3 

750  * INITIAL  TRAJECTORY 
760  IF ( KT  NE  0)  WRITEOCT,  200) 

770  200  FORMAT < IX, "TICS" ) 

780  CALL  TTARG 

790  * MEASUREMENT  PARAMETERS 
800  * RADAR  RANGE  S D 
810  SIGR=5 

820  * RADAR  CROSS  RANGE  ANGLE  S D 
830  SIGAZ=  001 

840  * I MU  ANGULAR  RESOLUTION 

850  SIGANG=  002 

860  * RATE  GYRO  RATE  S D 

870  SIGBET=  005 

8S0  QBET< 1 )«SIGBET**2 

890  QBET ( 2 ) =0 

900  * SIGANG-O.  FOR  THUTH  MODEL  REFERENCE 
910  IF<kSUB<  1 ).  EQ.  1)  SIGANG=0. 

920  IF(kSUB( 10).  NE  1)  GO  TO  310 

930  * PARAMETER  MODIFICATION  CAPABILITY 

940  PRINT,  "SIGR, SIGAZ, SIGANG,  SIGBET" 

950  READ, SIGR,  SIGAZ,  SIGANG,  SIGBET 

960  QBET ( 1 ) *SI GBET *#2 

970  310  CONTINUE 

980  * INITIAL  COVARIANCES 

990  DO  1 IR=1 , 6 

1000  DO  1 IC*1, 6 

1010  PTI  < IR,  10=0 

1020  1 PTL<IR.  10=0. 

1030  PTL  < 1,  1 >=5  **2 

1040  PTL <2,  2)=<XTL< 1 )*S1GAZ)**2 

1050  PTL (3,  3)  = <300  )**2 

1060  PTL (4,  4 ) = < 300  )**2 

1070  PTL(5,  5)*=<  20  )**2 

1080  PTL (6,  6 ) = < 200.  )**2 

1090  * TRANSFORMED  FROM  LOS  TO  I 

1100  CALL  TPTRAN(2 ) 

1110  * STATE  NOISE  PARAMETERS 
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1120  * SPECTRAL  S D 
1130  SIGAC=2 

1140  * MARKOV  TIME  CONSTANT 
1150  TCAC=5 

ll60  * ICS  WRITTEN  TO  FILE  STM  TO  FORM  TRUTH  MODEL  IC'S 

1170  WRITE ! 4 ) XTI , XTL,  XTRAJ,  NS,  NM,  ND,  T,  DT,  TMX,  NT 

1180  WRI TE<  4 )PT I , PTL,  A, AD,  TM,  TMF,  SIGR,  SIGAZ,  SIGANG 

1190  WRITE! 4)R,  SIGAC,  TCAC,  QBET.  KFORM 

1200  * DIAGNOSTIC  PRINT 

1210  IF < KT  NE  1)  RETURN 

1220  WRITE!KT,  201 ) 

1230  201  FORMAT! IX,  "PTL" ) 

1240  DO  101  IR= 1 , NS 

1250  101  WR I TE ! KT , 1 00 ) ! PTL ! I R,  I C ) , I C* 1 , NS ) 

1260  100  FORMAT! IX,  6! 1PE1 1 3) > 

1270  WRITE!KT,  202) 

1280  202  FORMAT! IX,  "PTI" ) 

1290  DO  102  IR=1,  NS 

1300  102  WRITE!KT,  100)!PTI!IR,  IC),  IC**1,NS> 

1310  CALL  TPTRAN! 1 ) 

1320  WRITE!KT,  203) 

1330  203  FORMAT! IX,  "PTL  RECOVERY  CHECK") 

1340  DO  103  IR= 1 , NS 


* 

i 
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1350  103  WRITE!KT,  100) !PTL! IR,  IC),  IC=1, NS) 
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100  * R DEMOYER  1978 
110  SUBROUTINE  TICS(KFORM) 

120  COMMON/ TST/ XT  I < 6 > , XTL<6) 

130  COMMON/ TRAJ/XTRAJ< 2,  3) 

140  COMMON /NPAR/ NS,  NM, ND 
150  C OMMON / T I ME / T , DT, TMX , NT.  TF 
160  COMMON./ COV/PT I < 6.  6),  PTL<6,  6) 

1 70  COMMON / TRANS/ A,  AD.  TM  < 2,  2 ) , TMF  < 6,  6 ) 

180  COMMON/ RPAR/S I GR,  SIGAZ,  SI GANG. R<2,  2) 

1«0  COMMON/ QPAR/ S I GAC , TCAC, QBET ( 2 ) 

200  COMMON/ TEST /KT 

2 1 0 COMMON/ SUB/ KSUB  < 20 ) , KSO,  KTRANS 
220  DATA  kSUB/20*0/ 

23 0 * DIAGNOSTIC  PRINT  FILE  NUMBER 
240  PRINT, "KT"  £ 

250  READ, KT 

260  * ICS  FROM  SUB-OPTIMAL  RUN 
270  READ  ( 4 ) XT  I , XTL,  XTRAJ.  NS,  NM,  ND,  T,  DT,  TMX.  NT 
280  READ < 4 ) PT I , PTL, A,  AD,  TM, TMF,  SIGR, SIGAZ,  SIGANG 
290  READ<  4 )R, SIGAC, TCAC, QBET,  KFORM 
300  * KSO= 1 FOR  TRUTH  MODEL 
310  KS0=1 
320  KF0RM=3 
330  CALL  TTARG 

340  # SIGANG=0  FOR  TRUTH  MODEL 
350  SIGANG=0 
360  RETURN 
370  END 
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100  * R DEMOVER  1*73 

110  * TARGET  MODEL 

120  SUBROUTINE  TTARG 

130  COMMON/ TST/ XT  I (6),  XTL<6) 

140  COMMON/ TRAJ/XTRAJ< 2, 3) 

150  COMMON /NPAR/ NS,  NM.  ND 

160  COMMON /T I ME  / T I . DT. TMX. NT, TF 

170  COMMON /TRANS /A,  AD,  TM<2,  2),  TMF  < 6,  6) 

130  COMMON/ TEST /RT 

1*0  COMMON / NTM / TNMF ( 6, 6) 

200  DO  10  IR= 1 , NS 
210  DO  10  IC*1,  NS 
220  TMF  < 1R,  10=0 
230  10  TNMF  i IR,  10=0 

240  * CONSTANT  ACCELERATION  TRUE  TRAJECTORY 

250  • ID  = X,  Y DIRECTION 

260  * N~ 1 CURRENT  TIME 

270  * N=2  NEXT  TIME 

280  DO  20  N= 1 , 2 

2*0  IF < N EQ  1 ) T*T I +DT 

300  IF<N  EQ  2)  T= r I 

310  * X T I INERTIAL  STATE  VECTOR 

320  DO  1 ID=1, ND 

330  * POSITION 

340  XT  I < I D ) =XTRAJ< ID,  1 >+XTRAJ< ID, 2>*T+  5*XTRAJ( ID, 3>*T**2 
350  * VELOCITY 

360  XT  I ( ND* ID ) =X  TRAJ< ID. 2>+XTRAJ< ID. 3)*T 

370  * ACCELERATION 

380  1 XTI ( 2*ND+ID)=XTRAJ\ ID,  3) 

3*0  X = XTI ( 1 ) 

400  XD=XTI(ND+1) 

410  Y = X T 1(2) 

420  YD=XT I < ND+2 ) 

430  • AZIMUTH 
440  A=ATAN2  < Y,  X ) 

450  PI =3  1415*26 

460  IF (A  LT  0 ) A=A+2*PI 

470  * AZIMUTH  RATE 

480  AD=<X*YD-Y*XD)/(X**2+Y**2) 

4*0  ♦ INERTIAL  TO  LOS  TRANSFORMATION 
500  TM< 1, 1 >=X/SQRT(X*#2+Y**2> 

510  TM<  2, 2>=TM< 1,  1 ) 

520  TM< 1, 2)»Y/SQRT(X**2+Y**2) 

530  TM<  2,  1 ) =-TM< 1, 2) 

540  * FULL  TRANSFORMATION  3 PARTITIONS 

550  DO  2 IP-1,  3 

560  DO  2 I R= 1 , ND 

570  DO  2 IC= 1 , ND 

580  IRF=  < IP- 1 ) *ND+ 1 R 

5*0  ICF=< IP-1 >*ND+IC 

600  IF (N  EQ  1)  TNMF< IRF,  JCF)=TM< IR,  IC> 
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610  2 IF ( N EQ  2)  IMF < IRF, ICF )=TM! IR, IC) 

620  20  CONTINUE 

630  * INERTIAL  STATE  TRANSFORMED  TO  LOS  STATE 
640  DO  3 I R= 1 , NS 
650  XTL < IR ) -0 
660  DO  3 I RC= 1 , NS 

670  3 XTL! IR)=XTL! IR)+TMF! IR,  IRC)*XTI ! IRC) 

680  * DIAGNOSTIC  PRINTS 
690  IF < KT  NE  1)  RETURN 
700  WRITE!KT, 200) 

710  200  FORMAT ( IX,  "TTARG'' ) 

720  WRITE!  KT , 201 ) 

730  201  FORMAT  < IX,  " X T I " ) 

740  WRITE! KT,  100) XTI 

750  100  FORMAT  ! 1 X , 6 ! 1 PE  11.  3 ) ) 

760  WRITE!KT,  202) 

770  202  FORMAT< IX,  "XTL"  ) 

780  WRITE! KT , 100) XTL 
790  WRITE(KT,  203) 

800  203  FORMAT! IX, "A, AD" ) 

810  WRITE!KT,  100) A, AD 
820  WRITE!KT,  204  ) 

830  204  FORMAT! IX, "TM" ) 

840  DO  101  IR=1 , 2 

850  101  WRITE!)  T,  100)  !TM!  IR,  IC),  IC=1, 2) 

860  RETURN 
870  END 
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100  * R DEMGYER  1978 

110  * TRANSFORMATION  OF  COVARIANCE  MATRIX 
120  SUBROUTINE  TPIRAN(KF) 

130  * KF= 1 INERTIAL  TO  LOS 
140  * KF=2  LOS  TO  INERTIAL 
ISO  COMMON/COV/PTI  (6,  6),  PTL  <6,  6) 

160  COMMON/TRANS/ A.  AD,  TM<2,  2), TMF  < 6. 6) 

170  COMMON /NF'AR/ NS,  NM,  ND 
180  DIMENSION  TT<6,  6) 

190  DOUBLE  PRECISION  S 
200  GO  TO  (100, 200), KF 

210  * COVARINACE  TRANSFORMATION  INERTIAL  TO  LOS 

220  * PTL  = (TMF) <PTI ) (TMF)T 

230  100  CONTINUE 

240  DO  1 IR=1,  NS 

250  DO  1 IC=IR, NS 

260  S=0 

270  DO  2 IRC 1 = 1,  NS 
280  DO  2 I RC2= 1 , NS 

290  IF  < TMF  < IR,  IRC  1 ) EG  0 > GO  TO  2 

300  I F ( PT I ( I RC 1 , IRC2 ) EG  0 ) GO  TO  2 

310  IF(TMF(IC,  IRC2)  EG  0.  ) GO  TO  2 

320  S=S+TMF< IR,  IRC1 )*PTI ( IRC1,  IRC2)*TMF< IC,  IRC2) 

330  2 CONTINUE 

340  PTL < IR,  IC)=S 

350  1 PTLUC,  IR)=S 

360  RETURN 

370  * LOS  TO  INERTIAL 
380  200  CONTINUE 
3Q0  DO  11  I R= 1 , NS 
400  DO  11  IC=IR,  NS 
410  S=0 

420  DO  12  IRC 1 = 1,  NS 
430  DO  12  IRC2=1, NS 

440  IF(TMF< IRC1, IR)  EG.  0 ) GO  TO  12 

450  IF < PTL < IRC  1 , IRC2)  EQ  0.  ) GO  TO  12 

460  IF(TMF( IRC2,  IC)  EG.  0 ) GO  TO  12 

470  S=S+TMF  < I RC 1 , I R > *PTL < I RC 1 , IRC2 ) *TMF < I RC2,  IC ) 

480  12  CONTINUE 

490  PTI ( IR,  IC)=S 

500  11  F T I < IC,  1 R ) =S 

310  RETURN 

520  END 
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100  * R.  DEMOYER  1978 

110  * COMPUTATION  OF  STATE  NOISE 

120  SUBROUTINE  TQ(KFORM) 

130  COMMON/TST/XTI <6) , XTL<6) 

140  COMMON/NPAR/NS,  NM,  ND 

150  COMMON/TIME/T, DT , TMX, NT/ TF 

160  COMMON/TRANS/A/  AD/ TM<2, 2),  TMF(6, 6) 

170  COMMON/QPAR/SI GAC/  TCAC,  QBET <2) 

180  COMMON/TEST/KT 
190  COMMON/QNO I SE/Q  ( 6/  6) 

200  COMMON/SUB/KSUB ( 20 ) , KSO/  KTRANS 
210  DIMENSION  G<6) 

220  DO  1 IR=1,  NS 
230  DO  1 IC=1  / NS 
240  1 Q<IR,  10=0. 

250  * QU  = ALONG  AXIS  SPECTRAL  JERQUE  TERM  FROM 

260  * MARKOV  ACCELERATION  PARAMETERS 

270  QU=2.  *S I GAC**2/TCAC 

280  * QV  = CROSS  AXIS  TERM 

290  QV=4.  *QU 

300  * EQUAL  SPECTRAL  SUB  OP 

310  I F < KSUB  < 7 ) EG.  1 ) QU=.  5*  < QU+QV ) 

320  I F ( KSUB  ( 7 ) EG.  1 ) QV=QU 

330  GO  TO  ( 100/ 200/ 300,  400),  KFORM 

340  * TRANSFORM  VX, VY  TO  VS, VL 

350  200  VS=TM< 1,  1 )*XTI <3)+TM( 1,  2)*XTI(4) 

360  VL=TM < 2,  1 ) *XT I < 3 ) +TM ( 2, 2 ) *XT I ( 4 ) 

370  GO  TO  150 

380  * VS, VL  FROM  STATE  VECTOR 
390  100  VS=XTL<  3) 

400  VL=XTL  < 4 ) 

410  150  V1=VS 
420  V2=VL 
430  GO  TO  600 

440  * VX,VT  FROM  STATE  VECTOR 
450  400  CONTINUE 
460  300  VX=XTI ( 3) 

470  VY=XTI ( 4) 

480  V1=VX 
490  V2=VY 

500  * TRANSFORMATION  OF  TARGET  FIXED  SPECTRAL  TERMS 

510  * TO  COMPUTATIONAL  FRAME 

520  600  VSQ=V1**2+V2**2 

530  Q( 5, 5 ) =DT*  < V 1 **2*QU+V2**2*QV ) /VSQ 

540  Q(6, 6)=DT*< V2**2*QU+V1**2*QV ) /VSQ 

550  Q<5,  6 ) =DT * ( V 1 *V2*QIJ— V 1 * V2*QV ) /VSQ 

560  Q(6, 5)=Q(5,  6) 

570  IF (KFORM  EQ  3)  GO  TO  500 
580  I F < KSUB ( 2 ) NE.  1)  GO  TO  500 
590  * G MATRIX  TERMS  FOR  LOS  FRAME 
600  G< 1 )=XTL<2) 
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610  0!  2 ) “-X  Tl  < 1 ) 

620  0< 3) *XTL ! 4 ) 

630  0<4)«-xn  < 3) 

640  G!5>«XTL<6> 

630  0( 6 ) *-XTl (3) 

660  DT2«DT**2 
670  DO  160  IR-l.NS 
680  DO  160  IOIR.  NS 

690  Q(IR.  IO-CMIR,  ICI-H3!  IR)*G!  IC)*QBET!kF0RM>*DT2 
700  160  Q< IC,  1R)-CH  IR,  1C) 

710  300  IF ! KT  NF  1)  RETURN 
720  * DIAGNOSTIC  PRINTS 
730  WRITE!KT. 301 > 

740  301  FORMAT (IX* "TO" ) 

730  WRITE!KT*  502) 

760  502  FORMAT ! IX,  "QU,  OV.  VS,  VI  , VX,  VY.  VI,  V2"  ) 

770  WRITEIhT,  303 )0U,  QV,  VS,  VL,  VX,  VY.  VI,  V2 
780  303  FORMAT < IX, 6( 1 PEI  1 3)) 

790  IF ( KFORM  EQ  1)  WRITEIKT, 504 ) 

800  304  FORMAT (IX,  "0" ) 

810  IF < KFORM  EG  1)  WRITE! KT,  503)8 
820  WR I TE ( KT, 305 ) 

830  505  FORMAT! IX, "0" > 

840  DO  506  IR»1 , NS 

830  306  WRITE!KT,  303)  <Q!  IR,  IC),  IOl,  NS) 

860  RETURN 
870  END 
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100  * R.  DEMOYER  1978 

110  * STATE  TRANSITION  MATRIX  COMPUTATION 
120  SUBROUTINE  TSTM< KFORM) 

130  COMMON/T I ME/T < DT , TMX,  NT, TF 
140  COMMON/STM/PHI <6,  6) 

150  COMMON/ TEST/KT 

160  COMMON/ TRANS/A, AD, TM(2, 2), TMF ( 6,  6) 

170  COMMON/NPAR/NS,  NM,  ND 

180  COMMON/ QPAR/S 1 0 AC , TCAC,  QBET < 2) 

190  DIMENSION  PH<6, 6) 

200  COMMON /NTM/TNMF ( 6,  6) 

2 1 0 COMMON/SUB/KSUB  ( 20 ) , KSO,  KTRANS 

220  DOUBLE  PRECISION  S 

230  SAVE  PH, PHI 

240  IF( T.  GT.  0.  ) 00  TO  50 

250  B=l.  /TCAC 

260  EBT =EXP ( -B*DT ) 

270  DO  10  IR— 1, NS 
280  DO  10  IC= 1 , NS 
290  10  PH(  IR,  10=0 
300  BT=B*DT 

310  * EXACT  INERTIAL  STATE  TRANSITION  MATRIX 
320  PH<  1,  1 ) =PH<  2,  2)=PH<3,  3)=PH(4,  4>  = 1. 

330  PH  < 1 , 3 ) =PH  < 2,  4 ) =BT 

340  PH < 1 , 5 ) =PH < 2,  6 ) = ( EBT+BT-1 . ) /B**2 

350  PH<  3,  5)=PH(  4,  6)  = ( 1.  -EBT ) /B 

360  PH  < 5, 5 ) =PH  < 6,  6 ) =EBT 

370  50  00  TO  ( 100,  100, 300),  KFORM 

380  * LOS  STM 

390  * PHL=  ( TMF ) * ( F'H ) * ( TMF ) T 
400  100  CONTINUE 

410  **  DIAGNOSTIC  PRINT  ELIMINATED  ** 

420  IF<KT  NE.  99)  GO  TO  150 

430  WR I TE ( 2,  1 1 1 ) KFORM 

440  111  FORMAT (IX,  "STM,  KFORM  = ",  13) 

450  WRITE ( 2,  1 12) 

460  112  FORMAT ( IX, "TMF" ) 

470  DO  113  IR=1, NS 

480  113  WRITE <2,  114)< TMF < IR,  IC) » IC=1 » NS) 

490  114  FORMAT  < X » 6 < 1PE 1 1 . 3)) 

500  WRITE ( 2, 115) 

510  115  FORMAT ( IX, "TNMF" ) 

520  DO  116  IR=1 , NS 

530  116  WRITE<2, 1 14) <TNMF< IR, IC), IC=1, NS) 

540  WRITE ( 2, 117) 

550  117  FORMAT ( IX, "PH" ) 

560  DO  118  IR=1,NS 

570  118  WRITE<2, 1 14) <PH< IR, IC), IC=1, NS) 

580  150  CONTINUE 

590  * APPROXIMATE  PHI  L SUB  OPS 

600  IF < KSUB< 3 ) EQ.  1)  GO  TO  160 
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610  * LOS  STM  TRANSFORMED  FROM  INERTIAL  STM 
620  * BY  ROTATION  MATRICES  AT  T=N  AND  N+l 
630  DO  101  IR=i , NS 
640  DO  101  IC* 1 , NS 
650  S=0 

660  DO  103  I RC 1 = 1 , NS 
670  DO  103  IRC2=1 > NS 

680  103  S=S+TNMF ( IR, IRC1 )*PH< IRC1, IRC2 ) #TMF< IC,  IRC2) 
690  101  PHI ( IR,  IC)=S 
700  GO  TO  500 

710  * SEE  SUBROUTINE  SUBOP  FOR  SUB-OPTIMIZATIONS 
720  160  IF (KSUB<  4)  EQ.  1)  CALL  SUB0P<4) 

730  IF ( KSUB (3)  EQ  1)  CALL  SUB0P<3> 

740  GO  TO  500 

750  * INERTIAL  STM 

760  300  IF  < T GT  0 ) GO  TO  500 

770  DO  301  IR=1 , NS 

780  DO  301  IC= 1 1 NS 

790  301  PHI < IR, IC)=PH( IR, IC) 

800  500  IF ( KT  NE  2)  RETURN 

810  WR I TE  < K'T ,501)  KFORM 

820  501  FORMAT < IX,  "STM,  FORM  = ",  12) 

830  DO  502  I R= 1 , NS 

840  502  WRITE<KT,  503HPHHIR,  IC>,  IC=1,NS> 

850  503  FORMAT  < 1 X , 6 ( 1 PE 1 1 3 ) ) 

860  WRITEIKT,  505) 

870  505  FORMAT < IX, "TMF " ) 

880  DO  504  IR=1 , NS 

890  504  WRITE<KT, 503) (TMF< IR, IC),IC=1,NS) 

900  RETURN 
910  END 
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100  * R DEMOYER  1978 

110  * PROPAGATION  OF  VARIANCE 

120  SUBROUTINE  TPROP( KFORM) 

130  COMMON/TIME/T, DT, TMX, NT. TF 
1 40  COMMON/COV/PT 1(6,6),  PI L ( 6, 6 ) 
1 50  COMMON/STM/PH 1(6,6) 

160  COMMON/QNOISE/Q( 6,  6) 

170  COMMON/NPAR/NS,  NM,  ND 


180  COMMON/TEST/KT 
1 90  COMMON/SUB/KSUB ( 20 ) , KSO, KTRANS 
200  DIMENSION  T 1 ( 6, 6 ) , T2 < 6, 6 ) 

210  DOUBLE  PRECISION  S 
220  * T1  = TEMPORARY  ARRAY 
230  DO  1 IR=1 , NS 
240  DO  1 IC=1 , NS 

250  IF(KFORM.  LT  3)  T1 ( IR,  IC)=PTL( IR,  IC) 

260  1 IF < KFORM  GE.  3 ) T1 < IR,  IC)=PTI ( IR,  IC) 

270  * (PHI >*(P)*(PHI >T 
280  DO  10  IR=1, NS 
290  DO  10  IC=IR, NS 
300  S=0 

310  DO  1 1 IRC 1*1, NS 

320  DO  11  IRC2=1 , NS 

330  IF(PHI(  IR,  IRC1  >.  EQ.  0.  ) GO  TO  11 

340  IF(T1 ( IRC1, IRC2)  EG  0 ) GO  TO  11 

350  IF(PHI  < IC,  IRC2).  EQ  0.  ) GO  TO  11 

360  S=S+PHI ( IR, IRC1 >*T1 < IRC1, IRC2)*PHI ( IC, IRC2) 

370  11  CONTINUE 

380  T2< IC, IR)=S 

390  10  T2( IR, IC)=S 

400  CALL  TTARG 

410  * STATE  NOISE  ADDED 

420  DO  20  I R= 1 . NS 

430  DO  20  IC=1 , NS 

440  IF ( KFORM  LT  3)  PTL ( IR,  IC )*T2( IR,  IC)+Q< IR,  IC) 

450  20  IF (KFORM  OF  3)  PT I ( I R,  IC ) *T2 ( IR,  IC) ♦Q( IR,  1C ) 

460  * SUB- OPTIMAL  OPTIONS 

470  I F ( KSUB ( 5 ) EO  1)  i ALL  SUB0P(5> 

480  I F < KSUB ( 6 ) EQ  1)  CALL  SUBOP \ 6 ) 

490  * COVARIANCE  TRANSFORMATION 
500  IF (KFORM  LT  3)  CALL  TPTRAN<  2 ) 

510  IF ( KFORM  GE  3)  CALL  TPTRAN< 1 ) 

520  IF (KT  NE  2)  RETURN 
530  * DIAGNOSTIC  PRINTS 
540  WRITEU  T.  100) KFORM,  DT 

550  100  FORMAT*  IX,  "TFROP,  - m | OT  F*  4) 

560  WR I TF  < K T , 101 ) 

570  101  FORMAT ( IX. “T 1 " 

580  DO  102  I R* 1 NS 

590  102  WRI  TF  < ) Tie  •,  t • ! : 1 . U . j 

600  103  FORMAT  * IX 
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610  WRITE  ! KT  , 104) 

620  104  FORMAT  < 1 X . " T2 " ) 

630  DO  105  IR*i,NS 

640  105  WR I TE < KT,  103) ! T2! IR, IC) » IC=1. NS) 
650  WRITE! KT , 106) 

660  106  FORMAT! IX, "PTL" ) 

670  DO  107  IR=1. NS 

680  107  WRITE!kT,  103)  !PTL!  IR,  1C).  IC*=1,  NS) 
690  WRITE!KT,  108) 

700  108  FORMAT! IX, "PTI") 

710  DO  109  IR=1 , NS 

720  109  WRITE!KT,  103)  !PTI  < IR,  IC).  IOl.  NS) 
730  RETURN 
740  END 
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100  * R DEMOYER  1978 

110  * COMPUTATION  uF  MEASUREMENT  VARIANCE  TERMS 
120  SUBROUTINE  TRMAT (KFORM) 

130  COMMON/TST/XTI (6) , XTL<6) 

140  COMMON/ NPAR/NS,  NM,  ND 

150  COMMON/ TRANS/ A,  AD,  TM<2,  2),  TMF<6,  6.) 

160  COMMON/ RPAR/S I GR,  SIGAZ, SIGANG, R<2, 2) 

170  COMMON/ TEST/KT 

1 80  COMMON/ SUB/KSUB < 20 ) , KSO,  K TRANS 
190  DATA  R/4*0  / 

200  DS=XTL < 1 ) 

210  DL=XTL ( 2 ) 

220  * RANGE 
230  R11=SIGR**2 
240  * CROSS  RANGE 

250  R22=  < S I GR*DL/ DS ) **2+ ( S I GAZ *DS ) **2 
260  IF ( KFORM  GE  2>  R22=R22+(SIGANG*DS)**2 
270  R<  1,  1 > =R 1 1 
280  R < 2, 2>=R22 

2^0  IF < KFORM  LT  3)  GO  TO  100 
300  * TRANSFORMATION  FROM  LOS  TO  I 
310  CA=COS<  A) 

320  SA=S I N ( A ) 

330  R ( 1 , 1 ) =R 1 1 *CA**2+R22*SA**2 
340  R ( 2,  2 ) =R22*CA**2+R 1 1 *SA**2 
350  R ( 1 , 2 ) =R  < 2»  1 )=CA*SA*(R1 1-R22) 

360  I F ( KSUB ( 6 ) EQ  1)  R<  1 , 2 ) =R<  2,  1 ) =0. 

370  * DIAGNOSTIC  PRINTS 

330  100  I F ( KT  NE  3)  RETURN 

390  WR I TE ( KT , 101) KFORM 

400  101  FORMAT < IX, "TRMAT,  KF0RM=",I3) 

410  WR I TE < KT , 102) 

420  102  FORMAT (IX,  "R1 1, R22" ) 

430  WR I TE ( KT,  1 03 ) R 1 1 , R22 
440  1 03  FORMAT ( t X , 2 ( 1 PEI  1 3 ) ) 

450  WRITE ( KT, 104> 

460  104  FORMAT ( IX,  "R" ) 

470  DO  105  IR=1 , NM 

480  105  WRITE<KT, 103) <R( IR, IC), IC=1, NM) 

490  RETURN 
500  END 
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100  * R DEMOYER  1978 

110  * SUB-OPTIMAL  KALMAN  GAIN  COMPUTATION 
120  SUBROUTINE  SKGA I N < KFORM ) 

100  COMMON /TRANS /A, AD, TM<2,  2), TMF  < 6, 6) 

140  COMMON /NFAR/ NS,  NM,  ND 

150  COMMON /COV/PT I <6, 6), PTL<6, 6) 

1 6.0  COMMON  / RPAR  / S I OR,  S I GAZ , S I GANG,  R < 2,  2 ) 

1 70  COMMON / GA I N /KG  < 6, 2 ) 

180  COMMON/ TEST /KT 

1*0  COMMON/ SUB/ KSUG < 20 ) , KSO,  KTRANS 

200  DIMENSION  PP<  6, 6 ) , RM<  2, 2 ) , RMI (2*  2 ) , CG<  6, 2 ) 

210  REAL  KG 

220  DOUBLE  PRECISION  S 

230  * PP  = TIMPORARY  COVARIANCE  ARRAY 

240  DO  1 I R= 1 » NS 

250  DO  1 IC*1 • NS 

26.0  IF  (KFORM  LT  3'  PP(  IR,  IC)=PTL(  IR,  IC) 

270  1 IF < KFORM  GE  3)  PP< IR,  IC)=PTI < IR,  IC) 

280  * MEASUREMENT  COVARIANCE 

2*0  DO  2 I R= 1 • NM 
300  DO  2 I C= 1 , NM 

310  2 RM< IR, IC)=PP< IR, IC)+R< IR, IC) 

320  * INVERSE 

330  DFT -RM< 1,  1 ) *RM < 2,  2)-RM< 1, 2)*RM<  2,  1 ) 

340  RMI (1,1) =RM<  2,2) /DET 

350  RMI v 1 , 2 ) =-RM ( l , 2 ) / DET 

360  RM I < 2,  1 ) a-RM  ( 2,  1 ) ./  DEI 

370  RMI <2. 2)=RM( 1, 1 ) /DET 

380  * 1ALMAN  GAIN  IN  APPROPRIATE  FRAME 

3*0  DO  3 I R- 1 , NS 

400  DO  3 I C= 1 , NM 

410  S=0 

420  DO  4 IRC-1,NM 

430  4 S=S+PP< IR,  IRC)*RMI ( IRC,  IC) 

440  3 KG( IR. IC)=S 

450  IF  (KFORM  L.T  3)  GO  TO  200 

460  * KL«(TMF)*(KT >*<TM)T 

470  DO  110  I R- 1 , NS 

480  DO  110  IC=1 ■ NM 

4*0  S=0 

500  DO  111  I RC 1 = 1 • NS 
510  DO  111  I RC2* 1 • NM 

520  111  S=S+TMF ( IR,  IRC1 ) *KG< IRC  1 , IRC2) *TM< IC,  IRC2 ) 

530  1 10  CG< IR,  I C ) *S 

540  * INERTIAL  GAIN  WRITTFN  TO  FILE  BTM  DIRECTLY  FOR  KFORM 

550  WRI TF ( 4 )KG 

560  GO  TO  300 

570  200  CONTINUE 

580  * K I = ( TMF ) T*  < K L ) * ( TM ) 

5*0  * GAIN  TRANSFORMED  FROM  LOS  TO  INERTIAL  BEFORE  WRITING 
600  * TO  FILE  FOR  KFORM  =1  OR  2. 
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610  DO  210  IR=1,  NS 
620  DO  210  IC=1,NM 
630  S=0. 

640  DO  211  IRC1  = 1,NS 
650  DO  211  IRC2=1,NM 

*60  211  S=S+TMF< IRC1,  IR)*KG( IRC1, IRC2) *TM< IRC2,  IC) 

670  210  CG( IR, IC)=S 

680  WRITE<4)CG 

690  300  CONTINUE 

700  IF ( KT  NE.  3) RETURN 

710  WRITE<KT,  100) 

720  100  FORMAT < IX,  "COMPUTED  GAIN") 

730  DO  101  IR=1 , NS 

740  101  WRITE<KT,  102)<KG(IR,  IC),  IC=1,NM) 

750  102  FORMAT < IX,  6(  1PE1 1.3)) 

760  WRITE(KT, 301 ) 

770  301  FORMAT < IX, "TRANSFORMED  GAIN") 

780  DO  302  IR=1,  NS 

790  302  WR I TE ( KT , 102) <CG< IR, IC) , IC=1, NM) 

800  RETURN 
810  END 
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100  ♦ K DFMOYEK  IV 78 

110  * KALMAN  GAIN  REAP  FROM  FILE  BTM  FOR  TRUTH  MODEL 
120  SUBROUTINE  TKGA I N < KFORM ) 

130  COMMON/ TRANS/ A,  AD,  TM<2.  2),  TMF ( 6,  6) 

140  COMMON/NPAR/NS,  NM,  ND 
1 50  COMMON /COV / PT I ( 6, 6 ) , PTL < 6, 6 ) 

1 60  COMMON / RPAR / S I OR,  S I GAZ , S I GANG, R < 2,  2 ) 

170  COMMON/GAIN/KG <6, 2) 

ISO  COMMON/TEST/KT 

1 VO  COMMON/SUB /KSUB ( 20 ) , KSO, KTRANS 
200  REAL  KG 

210  * GAIN  READ  IN  INERTIAL  FRAME 
220  READ<  4) KG 
230  RETURN 
240  END 
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100  * R DFMOYER  1*78 
1 1 0 * A POSTE R 1 OR  1 C OV  AR I ANCE 
120  'SUBROUTINE  TPOST (KFORM) 

1 30  COMMON/ COV /PT 1 ( 6,  6 ) , PTL  < 6. 6 ) 

140  COMMON ,'NF'AR/  NS  NM.  ND 
1 50  COMMON /REAR  > S I OR , S I OAZ , S I GANG,  R < 2,  2 ) 

1 60  COMMON/ GA 1 N / KG < 6,  2 ) 

170  COMMON /TEST / KT 

1 80  COMMON/ SUB / KSUB ( 20 ) , KSO,  KTRANS 
190  DIMFNSION  P(6,  6),  IKH<6,  6),  PJ<6,  6) 

200  noup.l  F PRECISION  S 
210  REAL  KG, IkH 

220  * P = TIMPORARY  COVARIANCE  ARRAY 
200  DO  1 I R= 1 , NS 
240  DO  1 I C=  1 , NS 

250  IF ( KFORM  LT  3)  P<  IR,  IC ) =PTL  ( IR,  IC ) 

260  1 IF < KFORM  GE  3)  P< IR,  IC)=PTI ( IR,  IC) 

270  * < I— KG*H) 

280  DC*  2 I R=  1 , NS 
390  DO  2 I C- 1 , NS 
300  IKH<  IR,  10=0. 

310  I F < I R EG  IC)  IKH<  IR,  10=1 

320  2 IF< IC  LE  NM)  IKH< IR, IC)=IKH< IR, IC)-KG< IR, IC) 

330  IF (KSO  EG  1)  GO  TO  400 

340  * SHORT  FORM  A POSTERIORI 

350  * SUB-OPTIMAL  FORM 

360  * P=(I-KG*H)*P 

370  DO  10  I R= 1 , NS 

380  DO  10  J C= I R, NS 

390  S-0 

400  DO  11  IRC- 1,  NS 

410  11  S=S+IKH(IR, IRC)*P<IRC, IC) 

420  IF < KFORM  NE  3)  PTL( IC,  IR)=S 
430  IF (KFORM  NE  3)  PTL ( IR, IC)=S 
440  IF  (KFORM  EG  3)  PTKIC,  IR)=S 
450  10  IF  (KFORM  FQ  3)  PTKIR,  IC)=S 
460  GO  TO  200 

470  * TRUTH  MODEL  JOSEPH  FORM 

480  400  CONTINUE 

4*0  DO  20  I R= 1 , NS 

500  DO  ?0  IC-IR, NS 

510  3=0 

520  DO  21  IRC 1 = 1,  NS 
530  DO  2 J IRC 2=1, NS 

540  21  S=S+IKH( IR, IRC1 >*P< IRC1, IRC2)*IKH< IC, IRC2) 
550  DO  22  I RC 1=1, NM 
560  DO  22  IRC2=1 , NM 

570  22  S=S+KG( IR,  IRCl )*R( IRC1,  IRC2)*KG< IC,  IRC2) 

580  PJ< IR,  IC)=S 

5*0  IF ( KFORM  NE  3)  PTL ( IC, IR ) =S 
600  IF  < KFORM.  NE  3)  PTL  ( IR,  IC)=S 
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610  IF  ( KFORM  FQ  1)  PTKIC,  IR)=S 
620  I F ( KFORM  EO  3)  PTKIR,  IC)=S 
6 ^O  20  PU( TO,  1 R ) — S 
640  200  CONTINUE 

650  * SFF  SUBROUTINE  SUBOP  FOR  SUV-OPTIMAL  CODES 
660  I F < kSUR < 5 ) EO  1)  CALL  SUB0P<5) 

670  IF  < KSUB( 6)  EO  1)  CALL  SUB0P<6) 

680  IF  ( kFORM  LT  ^?)  CALL  TPTRAN<2) 

6*0  IF (KFORM  GE  3)  CALL  IF TRAN < 1 ) 

700  IFvKT  NE  3>  RETURN 
710  * DIAGNOSTIC  PRINTS 
720  WK 1 TF  < k T,  100> 

730  100  FORMAT (IX, "A  PRIORI  COVARIANCE") 

740  DO  101  I R= 1 , NS 

750  101  WRITE(KT  102) <P< IR,  IC),  IC=1, NS) 

760  103  FORMAT ( IX, 6( 1 PEI  1 3)) 

770  WRITE (FT, 103) 

7S0  103  FORMAT< IX, "A  POSTERIORI  COVARIANCE") 

7*0  DO  104  IR* 1 , NS 

800  IF (KFORM  LI  3»  WRITEIKT,  102) (PTL( IR.  IC) » IC=1»  NS) 

810  104  IF < KFORM  GE  3)  WR I TE < KT,  1 02 ) ( PT I ( I R.  IC ) , IC= 1 , NS ) 
820  WR1TE<KT. 110) 

8 30  110  FORMAT (IX,  "TRANSFORM  CHECK") 

840  DO  in  IR*)  , NS 

850  IF (KFORM  LT  3)  WR1 TE ( K T,  1 02 ) < PT I ( IR.  IC ) , IC* 1 , NS ) 

860  111  IF (KFORM  GE  3)  WR I FE ( KT,  1 02 ) ( PTL ( IR,  IC ) , IC= 1 , NS ) 

870  WRITE <F T . 1051 

880  105  FORMAT (IX. "JOSEPH  FORM") 

S*0  DO  106  IR=1 . NS 

*00  106  WRITEOT,  102)  <PJ(  IR.  IC),  IC=1,  NS) 

*10  RETURN 
*20  FNri 
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100  * R DE MOVER  1^78 

110  * PROPAGATION  OF  VARIANCE  TO  TIME  IF  IMPACT 
120  SUBROUTINE  SPROJ 
130  COMMON /NPAR/'NS,  NM,  NP 
140  COMMON/ TST/ XT  I <6>,  XTL<6> 

ISO  COMMON /COV/PT I (6, 6) , PTL<6, 6) 

1 6.0  COMMON  / OPAR  / S I GAC , T C AC . QBE  T ( 2 ) 

170  COMMON /PCV/ PSP <2, 2), PTP<2, 2) 

180  COMMON/ TEST /KT 

1O0  COMMON / SUB /RSUBI 20),  KSO,  k TRANS 
200  COMMON/TIMF/T. DT. TMX, NT, TF 
210  DIMFNSION  PH(6>  6) » P(6,  6>) 

220  DOUBLE  PRECISION  S 
230  DATA  BV, CP/ 1200  , 007/ 

240  B = 1 /TCAC 
250  TF  = 01 

260  CBV=  5*CP*SQRT(BV> 

270  « TIMF  OF  FLIGHT  ITERATION 
280  1 BT=B*TF 
290  EBT~E XP < — BT ) 

300  TFP=TF 

310  XP=XTI ( 1 >+XTI < 3)*TF+XTI < 5 > * < EBT+BT- 1 )/B**2 
320  YP=XTI<2>+XTI<4)*TF+XTI<6)*<EBT+BT-1  )/B**2 
330  RP=SQRT< XP**2+YP**2> 

340  TF-RP/ < BV-RP*CBV ) 

350  IF  < ABS<  TF-TFP ) GT  01)  GO  TO  1 

360  * SPFCTRAl  TERMS 

370  QU- 2 *S I GAC* *2 /TCAC 

380  0V=4  *0U 

3°0  VX=XT? <3) 

400  VV«XTl<4) 

410  VS0*VX**2+VY*»2 

470  055= ( VX **2*0U+VV**2*QV ) / VSQ 

430  066=  < VY **2*0U+VX**2*0V ) / VSQ 

440  0561*  ( VX  *VY*OU-VX  *VY*QV ) ./  VSO 

450  N l = 1 + 2*TF 

460  DTF=TF'NI 

470  FT=B*DTF 

480  EBT =F  XP  < -BT ) 

480  * PROPAGATION  OF  COVARIANCE  IN  INERTIAL  CLOSED  FORM 

500  DO  2 1 I R= 1 , NS 

510  DO  21  IC=1,  NS 

520  P< IR. I C ) =PT I < IR, IC> 

530  21  PH ( IR  IC  > =0 

540  PH( 1, 1 )=PHi 2, 2)»PH<3. 3)=PH<4. 4)=1 

550  PH< 1, 3>=FH<2. 4)=DTF 

560  PH( 1 , 5>=PH<2, 6>=<EBT+BT-1.  ) /B**2 

570  PH ( 3,  5 ) =PH  < 4 , 6 ) = ( 1 -EBT ) /B 

580  PH <5.  5)=*PH<  6,  6)=EBT 

580  DO  40  1 = 1,  NI 

600  DO  10  I R= 1 . NS 
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610  DO  10  IC=IR, NS 
620  S=0 

630  DO  1 1 I RC 1 = 1, NS 
640  DO  11  IRC 2=1,  NS 

650  11  S=S+PH! IR,  IRC  1 >*P< IRC  1 , IRC2 >*PH< IC,  IRC2) 

660  I F ( I R EQ  5 AND  IC  EQ  5)  S=S+Q55*DTE 

670  I F < I R EQ  5 AND  IC.  EO  6)  S=S+Q56*DTF 

680  I F < I R EQ  6 AND  IC  EQ  6)  S=S+Q66*DTF 

690  P!  IR, IC)=S 
700  10  P< IC, IR)=S 
710  40  CONTINUE 

720  * PROJECTED  POSITION  COVARIANCE  OF  OPTMAL  FILTER 

730  * WRITTEN  TO  FILE  REFF 

740  DO  41  I R= 1 , 2 

750  DO  41  IC-IR, 2 

760  41  PSP( IR, IC)=P( IR, IC) 

770  IF ( KSUB ( 1 ) EQ  1)  WRITE(6)PSP 
780  I F ! KT  NE  2)  RETURN 
790  WR I TF ( 2,  100) 

800  100  FORMAT (IX,  "SPROJ" ) 

810  WR I TF  < 2,  101 ) TF 

820  101  FORMAT (IX,  “TF^*,  1PE1  1 3) 

830  WRITE (2, 102) 

840  102  FORMAT! IX, "PSP") 

850  DO  103  IR=1,NP 

S60  103  WRITEC2,  104)  <PSP<  IR,  IC),  !C=1,  ND) 

870  104  FORMAT!  IX,  6<  1PE1  1.  3)  ) 

880  RFTIIRN 
8°0  END 
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100  * R.  DEMOYER  1978 

110  * PROPAGATION  OF  COVARIANCT  TO  TIME  OF  IMPACT 
120  SUBROUTINE  TPROJ 
130  COMMON/NPAR/NS,  NM/  ND 
140  COMMQN/TST/XTI < 6> , XTL(6) 

1 50  COMMON/COV/PT  1(6,6),  PTL  (6,6) 

160  COMMGN/QPAR/S I GAC,  TCAC, GBET ( 2) 

1 70  COMMON/PCV/PSP  (2,2),  PTP  (2,2) 

180  COMMON/TEST/KT 

190  COMMON/T I ME /T , BT, TMX, NT, TF 

200  DIMENSION  PH( 6, 6 ) , P ( 6, 6 ) 

210  DOUBLE  PRECISION  S 
220  DATA  BV,  CP/ 1200.  007/ 

230  B=l.  /TCAC 
240  TF=.  01 

250  CBV=.  5*CP*SQRT(BV) 

260  * TIME  OF  FLIGHT  ITERATION 
270  1 BT=B*TF 
280  EBT=EXP(-BT) 

290  TFP=TF 

300  XP=XTI < 1 )+XTI (3)*TF+XTI (5)*(EBT+BT-1.  )/B**2 
310  YF’=XT I ( 2 ) +XT I (4) *TF+XT I ( 6 ) * ( EBT+BT— 1 . )/B**2 
320 ♦ RP=SQRT ( XP**2+YP**2 ) 

330  TF=RP/ ( BV-RP*CBV ) 

340  I F ( ABS  ( TF-TFP ) . GT.  01)  GO  TO  1 

350  * SPECTRAL  TERMS 

360  QU=2.  *S I GAC**2/TCAC 

370  QV=4.  *QU 

380  VX=XTI (3) 

390  VY=XTI ( 4 ) 

400  VSQ=VX**2+VY**2 

4 1 0 055=  ( VX**2*GU+VY**2*QV ) /VSG 

420  Q66= < VY**2*QU+VX**2*QV ) /VSG 

430  056= ( VX  *VY*QU-VX  *VY*QV ) /VSG 

440  NI=1+2*TF 

450  DTF=TF/NI 

460  BT=B#DTF 

470  EBT=EXP(-BT) 

480  * PROPAGATION  OF  COVARIANCE  IN  INERTIAL  CLOSED  FORM 

490  DO  21  I R= 1 , NS 

500  DO  21  IC=1,  NS 

510  P(IR. IC ) =PT I ( IR, IC) 

520  21  PH ( IR,  10=0. 

530  PH(  1,  1 )=PH(2,  2 ) =PH ( 3,  3>=PH(4,  4>  = 1. 

540  PH  (1,3)  =PH  (2,4)  =DTF 

550  PH ( 1 , 5 ) =PH ( 2, 6 ) = ( EBT+BT- 1 . ) /B**2 

560  PH ( 3, 5 ) =PH (4, 6 ) = ( 1 . -EBT ) /B 

570  PH  ( 5,  5 ) =PH  (6,6)  =EBT 

580  DO  40  1 = 1,  NI 

590  DO  10  1R=1 , NS 

600  DO  10  1C=IR, NS 
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810  S=0 

620  DO  1 1 IRC  1 = 1. NS 
630  DO  11  IRC2=t » NS 

640  1 1 S~S+PH( IR,  IRC1 )*P< IRC1,  IRC2)*PH( IC,  IRC2) 

650  IF < IR  EO  5 AND  IC  EQ.  5)  S=S+Q55*DTF 

660  IF<  IR  F.Q  5 AND  IC.  EQ  6)  S=S+Q56*DTF 

670  IF < IR  EQ  6 AND  IC  EQ  6)  S=S+Q66*DTF 

680  P<  IR,  IO-S 

690  10  P< IC, IR ) =S 

700  40  CONTINUE 

710  DO  41  IR= 1 , 2 

720  DO  41  IC=IR,  2 

730  41  PTP< IR,  IC)=P(IR,  IC) 

740  * PROJECTED  OPTIMAL  POSITION  COVARIANCE  READ  FROM  FILE  REF 

750  READ( 6) PSP 

760  I F < K T NE  2)  RE  TURN 

770  WRITE <2,  100) 

780  100  FORMAT < IX, "TPROJ" ) 

790  WR I TE  < 2,  101 ) TF 

800  101  FORMAT ( 1 X,  "TF«*"»  1PE1 1 3) 

810  WR I TE < 2,  102) 

820  102  FORMAT < 1 X,  "FTP" ) 

830  DO  1 0.3  I R=  1 , ND 

840  103  WRITE <2, 104) <PTP< IR, IC), IC=1,  ND) 

S50  104  FORMAT < IX,  6< 1PE1 1.3)) 

860  RETURN 
870  END 

880  * EVALUATION  SUBROUTINE 
880  SUBROUTINE  TEVAL(KF) 

800  COMMON  / PCV  / PSP  (2,2),  PTP  (2,2) 

910  COMMON/TIME/T,  DT,  TMX,  NT,  TF 
8?0  SAVE 

830  IF<  T GE  2 ) GO  TO  50 
°40  SSS=SST=TN=0 
850  N=0 

860  IF ( T LT  2 ) RETURN 

870  50  GO  TO  (100, 200), KF 

880  100  RSPS=SQRT(PSP( 1, 1 )+PSP(2, 2) > 

880  RSPT  =-SQRT ( PTP ( 1 , 1)+PTP(2,  2)) 

1000  * SSS  = OPTIMAL  SUM  SQUARE  POSITION  ERROR 
1010  SSS«SSS+PSP( 1,  1 )+PSP(2, 2) 

1020  * SST  = TRUTH  MODEL  SUM  SQUARE  POSITION  ERROR 
1030  SST=SST+PTP( 1 , 1>+PTP(2, 2) 

1040  N=N+ 1 

1050  TH-TH+-RSPS/RSPT 
1060  RETURN 
1070  200  TH-TH/N 
1080  RMSS*SQRT ( SSS/N ) 
lO^O  RMST -SORT ( SST/N ) 

1 100  PRINT  201, TH 

1110  201  FORMAT (IX, "PC  * ",F10  6) 
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100  * R DFMOYER  19  78 

110  * PLOT  SUBROUTINE  — DATA  WRITTEN  TO  FILE  BPLT 

120  * FOR  SUBSEQUENT  CRT  PLOTTING 

180  SUBROUTINE  TPl.OT 

140  COMMON/  l IMF/T,  DT,  TMX.  NT..  TF 

1 ■SO  COMMON /TST/XTI  (6),  XTL<6) 

160  COMMON/ TRANS/ A,  AD,  TM<2,  2),  TMF ( 6,  6) 

170  COMMON /RPAR/S I GR, SIGAZ, SIGANG, R<2, 2) 

1 80  COMMON/ QNO 1 SE / Q ( 6, 6 ) 

1 80  C OMMON/GA 1 N/KG < 6, 2 ) 

200  COMMON /OOV/PT I <6,  6),  PTL < 6,  6) 

? 1 0 C OMMI W PCV / PSP <2,  2),  PTP (2,2) 

220  REAL  ) G 

230  DIMENSION  V< 105) 

240  N-0 

250  no  1 1 = 1.6 

260  N=N+t 

270  t V(N)=XTL< 1 ) 

280  DO  2 1=1,6 

2^0  N'-N  ♦ t 

300  2 V(N)=XTI < I ) 

310  V < 13) =A 
320  V<14)=AD 
330  V ( 1 5 » «R < 1 , 1 ) 

340  V ( 1 6 ) *R  < 1 , 2 ) 

350  V( 17)=R(2,  2) 

360  M= 1 7 
370  DO  3 JR=1,6 
380  DO  3 IC=  IR,  6 
390  N=N+1 

400  3 V(N)=Q( IR, IC) 

410  DO  4 I R= 1 , 6 
420  DO  4 1 o ~ ■ i , 2 

430  N“N ♦ 1 

440  4 V(N>=KG< IR, IC) 

450  DO  5 1 R - l . 6 
4 AO  DO  5 I C «■  1 R , <s 
470  N-N»l 

480  5 V < N > PTL < IR,  IC) 

490  DO  S IR=1 , 6 
500  DO  6 I C “ I R , 6 
510  N=N+ 1 

520  6 0(N)=PT I ( IR, IC) 

530  V<92)=PSF< 1,1) 

540  V(94)  PSP  < 1 , 2 ) 

550  V < 85  > -psp  (2,2) 

560  V < 86 > =F'TP  ( 1,1) 

570  0(87  > r.RTPf  1,2) 

580  V( 8ft> -FTP ( 2,  2) 

590  V<  9‘  . ) --  TF 
600  V ( 1 05 ) *7 


*4.'  * V<  I ) •> 

■ • ’ * '.11  . »•  ' l AL  r A 1 • V i 1 1 

A.*, Ci  ♦ 9<  > 2< 

67A  » A AZI  iH  : 4?;n«  ' 

A ;.f.  » V < I 5 » , V < ‘ 

6~0  * R MF  ASI  iF  •' 

700  * 14  I a. 

710  * 17 

720  * " - 1 All  NO  I F . i> 

730  * |.:  )v  20  2 1 ».  2 _• 

740  • 24  „-S  ?A.  2 7 28 

750  * 2°  50  51  32 

76-0  * 35  54  35 

7 70  * 36.  37 

780  * 38 

7-50  * n * 1 Al  MAN  OAJN  IE  KM1-. 

800  * 3-'  40 

810  * 41  42 

820  * 4 . 44 

830  * 45  4 A. 

840  * 47  48 

850  * 4-  SO 

860  « PTL  * LOS  COVARIANCE 

870  «■  51  52  53  54  55  56 

880  * 57  58  58  60  61 

880  * 62  68  64  65 

900  * 66  67  68 

910  * 69  70 

820  * 7 1 

930  * FT  T = INERTIAL  COVARIANCE 

840  * 72  73  74  75  76  77 

950  * 7 8 79  80  81  S2 

960  * 83  84  95  86 


8 §0  « 90  81 

990  *•  92 

1000  * OPT t MAI  PROJECTED  POSITION  TERMS 

1010  * P9P~V<93, 84, 95) 

1020  * TRUTH  MODEL  PROJECTED  POST  I ION  TERMS 
1030  * RTF- V ( 96.  87,93) 

1040  * TF  ~ V< 99). TIME  OF  FLIGHT 
1 050  « T I MF  * V < 1 05 ) 

1060  RETURN 
1070  FND 
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1 I M > 

I to 

I 20 

» i 

i4o 

t«i 

1 AO 
• 70 
1 !?<" 
l«C 
700 
2 1 0 

2 20 
2 30 
->40 

25(1 
A O 
2 70 
80 

•:  , ( > 


• *•  i*  raw  k i 

• HI  i*  I IMIA7  I I UN  i I ImNS 

Ml  >.  ■ hi  1 i Nt  i III  Ip  ( K-.  I 

• I I Kl  f f El  Ni  I P 1 1 P WRI 1 TEN 

• I i till  ADD!  |i 

• > PHI  I = I ♦ FL*DT 

• > -1  Al ii  i OMPIITFP  BY  NUMERICAL  DIFFERENCE 

• l DP  i Ml  IPL  I NO  IN  LOS 

. l / riF  i HI  IF  L I NO  I N I NER  T I Al 

• f - , FOUAI  SPECTRAL  TERMS 

• l lO  l l‘:.FD  AS  HAM  TO  READ  ERROR  PARAMETERS 
i . -MMi  IN  / T IMF  / T.  DT  . TMX.  NT,  TF 

COMMi  IN  /STM  /PH  I <0,  6) 

i iimmmN/  TRAN':  /A,  AD.  1M<2,  2),  TMF  (6,  6) 

• >MMi  IN  / NEAR  / NS . NM . ND 

' i -1MUN  / 1 'PAR  / S I OAC  , TCAO , GBET  < 2 ) 

. MIS'  in  ■ REAR  / S I OR  SIGAZ,  SI  GANG,  R ( 2,  2 ) 

• OMMON  I IV/PTI  <6,  6),  PTL  < 6,  6) 

• iMM  iN  H[:/|  SI  ID ( 20),  I SO,  KTRANS 
■AW  AR 

: m i,l 00.  I SO, 200, 250. 250, 300 ) , KS 

• > l RE F F RFNOF  FILE  WRITTEN  ELSESHERE 

so  ,.r-  r i * • i 


* 1 OPE  T < 1 > ADDED  IN  TO 

40  ■>  ii  <N 

'*0  * PHT  ■-  I ♦ Fl*DI 
• ' l“i  P . / 1 1 AC 
70  D'  1 I R^  1 , NS 
• Tr.  • . ) i I C*  1 , NS 

••/<  ‘ Mt<  IR,  IC.1-0 

tOt  I F < I R EQ  1C)  PHI  <IR,  10  = 1. 

*'•  O PHI  ( 1 , s>=PHI  <2,  4)=PHI  <3,  5)=PHI  <4,  6)=DT 
420  PHI  < 1 . 2 ' “PH  I ( 3.  4>~.pHl  (5,  6)=AD*DT 
4 TO  PHI  I . ■ 1 i PH  I ( 4,  3)  “PHI  <6,  5)“-AD*DT 
440  PHI  <5.  5>=PH1  <6,  M = 1 -B*DT 
450  RETURN 

•1,  0 • ATI  MIMPIITFD  L-IY  NUMERICAL  DIFFERENCE 
4 70  200  IF <T  GT  0 > GO  TO  201 
4S0  AP-A 
4*0  RFTMRN 

500  * GAUSSIAN  RANDOM  NUMBER 

5 J 0 ’01  S*0 

520  DO  202  1 = 1 • 12 

530  702  S=S+(RRAND(1  )-  5) 

540  S=S/ 12 
550  AE  - A+S I GANG*s 
560  AD=  ( AE-AP  > / DT 
570  AP ~AF 
580  RFTURN 

5*0  * DECOUPLING  OF  COVARIANCE 
600  250  DO  751  IR=1,NS 
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DO  251  IC=IR,  NS 
DO  252  1 = 1/  5»  2 
IRF--IR+I 

IF<IF  EQ.  1.  AND  IC.  EQ  2.  AND.  KSUB<9).  EQ.  1)  GO  TO  252 
IF(IC.  EQ.  IRP.  AND.  KS.  EQ.  5)  PTL ( IR,  10=0 
I F < KSUB  ( 8 ) . EQ.  1 ) PTL  ( 1 , 2 ) =.  5 
IF < IC.  FQ.  IRP.  AND.  KS.  EQ.  6)  PTI  ( IR,  10=0. 

252  CONTINUE 

PTL  < IC,  IR)=F'TL  < IR,  IC) 

251  PTI < IC, IR ) =PT I < IR, IC) 

* FOR  KS=6,  R ( 1 , 2 ) =R (2,1)  =0.  IN  TRMAT 
RETURN 

* EQUAL  SPECTRAL  TERMS  IN  TQ 
300  RETURN 

END 
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100  * R DFMOYER  19/8 

110  * CONSTRUCTION  OF  ERROR  ELLIPSES  FROM  COVARIANCE  MATRICES 
120  DIMENSION  XE  < 82 ) , YE  < 82 ) , V < 105) 

1 30  OPEN!  ILE  8,  "PF" 

140  RFW1NP  8 
150  ENDFUF  6 

180  OPENFILF  5,  "BPLT",  "NUMERIC" 

170  2 REWIND  5 

ISO  PRINT. "TIME,  FUNCTION" 

1 90  READ,  T,  1 F 

/O'"*  1 READ (5,  END=2)V 

210  I F < T NE  V< 105) ) GO  TO  1 

220  • A PRIORI  POS,  VEL,  ACC  ELLIPSES  FOR  KF=1,2,  3 
2 30  * IMPACr  ELLIPSE  FOR  KF  = 4 
240  IF  OF  EC  4)  READ!  5) 

250  Cxi  ISO 
280  XT-V<7) 

->70  I F < X T EC*  0 > XI  = 00001 
280  YT  =V<  8 1 
2^0  9L-Y1/XT 

>00  GO  T t • <50  80,  70,  75,  80),  KF 
310  50  F'l  1 =V<  72 ) 

320  P 1 2«V < 73 ) 

830  P22=V<  78) 

340  PRINT, "A  PRIORI  POSITION  ELLIPSE" 

350  GO  TO  85 
380  80  P 1 1 =V ( 83 ) 

370  P12=V<84) 

380  P22--V(37) 

890  PRINT, "A  PRIORI  VELOCITY  ELLIPSE" 

400  GO  TO  85 
410  70  P11=V<90> 

420  P12-VC--I  i 
430  P22-V <-*/> 

440  PRINT. "A  PRIORI  ACCELERATION  ELLIPSE" 

450  GO  TO  :35 


480  75  Pit  -:V< 

470  P12=V<87> 

480  P22"V<9CO 

490  PRINT . "PROJECTED  TRU1H  POSITION  ELLIPSE" 

500  GO  TO  85 
510  SO  P 1 1 -V  < 93 ) 

520  P 1 2—V( 94 ) 

570  P22-V<95) 

540  PR TNT,  "PROJECTED  SUBOPT  I MAL  POSITION  ELLIPSE" 

550  85  CONTINUE 

580  * COVARIANCE  INVERSE 

5?0  OF  1 -R 1 1 *P22-P 1 2**2 

580  C11-P22/DET 

590  C 1 2~C 2 1 = -P 1 2 / DE  T 

800  C22*PI 1 < DET 
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MO  XLM=SGRT(C22*C**2/<C1 1*C22-C  12**2)  ) 

6.20  DX -XL. M/40 
630  N=0 

640  no  ?0  K= 1 , 2 
650  DO  20  J=l,41 

660  IFIK.F.Q.  1)  X=-XLM+< J-l >*DX*2 
670  I F < K.  EQ  2)  X=XLM-< J-l )*DX*2 
680  N=N+ 1 
690  XE<N)=X 

700  D 1 8=AE:S  ( X **2*  ( C 1 2**2-C- 1 1 *C22  ) +C22*C**2 ) 

710  IF(K  EG!  1)  YE  < N)  = < -X*C1 2-SQRT ( DI S ) ) /C22 

720  20  IF<K.  EQ.  2)  YE< N) =< -X*C12+SQRT < DIS) )/C22 

730  TERM-1  E 37 

740  DO  80  J-- 1 , 82 

750  30  UP I TE ( 6,  33 )XE<J),  YE < J ) 

760  33  FORMA T ( 1 X , 2 ( 1 PE 1 1 . 3 ) ) 

770  WRITE (6. 33) TERM, TERM 

780  DO  40  K-l , 2 

790  DO  41  .J-l,  4 1 

800  X ~-XLM+  < .J- 1 ) *DX*2 

810  XI.- 1 2*X 

320  1R|  EQ.  1)  YL=XL*SL 

830  IF < K FO  2)  YL=-XL/SL 

840  4 1 WP 1TE< 6, 33 ) XL, YL 

850  40  WR1 IE (6,  33) TERM,  TERM 

360  END 
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100  * R DF.MOYER  1978 

110  * GENERATION  OF  STANDARD  PLOTTING  FORMAT  FILE  FOR  L IG***  TEKGRAF 
170  DIMFN'-  IuN  A ( 10  700 ) • V < 1 05 ) , NON < 1 0 ) • T ( 200 ) 

130  OPF.NFIIF  1.  "PPL  T’\  " NUMERIC" 

140  REWIND  5 
1 50  OPFNF  ILb  «■  "PF  •• 

! 60  REWIND  r 
170  F.NDb  lib  <S 

IPO  KRINT.  "I*  AND  VARIABLES" 

1°0  READ  NV.  < NVN  < I > ..  I = 1 , NV > 

700  TERM  | E 37 
710  NT-0 

?•'  i re  AD  < *S . t ND  - 1 00  ) V 
7 30  N1--NT  + 1 
740  DO  7 I - 1 , NV 
’•*0  NN-NVN<  I ' 

?60  7 A < T ■ NT  > =V  < NN ) 

770  T \ NT  > *V  ( 1 05  ) 

780  GO  Tr'  1 

790  100  DO  101  1-1.  NV 

TOO  T>0  10.  N t , NT 

310  107  WRITER,  103)T<N>,  A<  1,  N) 

370  1 0 1 WRITE!  f>,  1 03  * TERM,  TERM 
330  10  F ORMAT < l X . 2 < t PE  11  3 ) ) 

340  F NDF  HE  6 
350  FND 
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claims  have  been  made  regarding  the  relative  advantages  of  statistical  de- 
coupling in  fixed  inertial  coordinates  as  compared  to  rotating  non-inertial 
coordinates.  An  error  analysis  model,  consisting  of  a sub-optimal  filter  and 
truth  model,  shows  the  error  added,  in  each  of  three  formulations,  due  to 
several  sub-optimal  approximations . Plots,  including  error  ellipses,,  help 
to  provide  an  intuitive  feel  for  the  results  of  decoupling.  ^ 
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